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ABSTRACT 


Underwater  aeoustie  networks  provide  an  interfaee  between  UUVs  and  surfaee  or 
land-based  eontrol  systems.  By  exploiting  range  data  measured  ineidental  to 
eommunieations  on  these  networks  it  is  possible  to  perform  underwater  positioning 
similar  to  that  of  the  satellite-based  GPS  program.  In  this  thesis,  several  algorithms  for 
generating  position  fixes  from  these  range  data  are  implemented,  tested,  and  evaluated 
with  synthetie  data.  The  algorithms  are  then  applied  to  data  obtained  during  operations  at 
sea. 
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EXECUTIVE  SUMMARY 


The  US  Navy  is  increasingly  using  unmanned  vehicles  for  operations  in  hostile 
environments.  Examples  of  such  missions  include  forward  reconnaissance  and  mine 
clearing  operations.  Robust  command  and  control  (C2)  infrastructure  and  precision 
guidance  are  vital  to  mission  success  when  using  these  remote  platforms. 

Acoustic  networks  have  been  developed  to  facilitate  C2  of  unmanned  underwater 
vehicles  (UUVs).  These  networks  can  also  be  used  to  improve  submarine 
communications  at  speed  and  depth  for  submarines  (SSNs)  operating  nearby. 
Additionally,  SSNs  and  UUVs  operating  with  such  a  network  can  be  used  as  intermittent 
gateways  for  fixed  sensor  assets  in  the  network.  In  this  role,  the  mobile  node  collects 
data  from  the  network  and  then  breaks  the  surface  to  transmit  large  data  dumps  to  an 
over-the-horizon  control  center,  eliminating  the  need  for  a  continuous  surface  presence. 
Accurate  knowledge  of  the  position  of  a  mobile  node  improves  operation  between  assets 
in  the  network  and  increases  network  efficiency  by  minimizing  the  routing  distance 
required  for  communications.  Ranges  calculated  incidental  to  network  communications 
provide  a  reliable  fix  source  using  already  available  modem  hardware. 

If  ranges  to  the  mobile  node  from  multiple  fixed  nodes  are  collected,  a  position 
fix  can  be  generated  in  a  manner  similar  to  a  terrestrial  receiver  using  GPS.  Several 
algorithms  for  solving  the  position  fix  are  compared  using  synthetic  range  data  to 
determine  their  relative  effectiveness.  These  tests  also  show  that  the  mean  and  standard 
deviation  of  the  resulting  position  error  are  linear  functions  of  the  standard  deviation  of 
the  error  in  the  measured  ranges. 

Operational  testing  with  the  Seaweb  acoustic  network,  currently  being  developed 
by  SPAWAR  Systems  Center,  San  Diego,  was  performed  to  collect  range  data  to  mobile 
nodes  operating  with  the  network.  Data  from  these  tests  are  analyzed  and  the  resulting 
position  fixes  show  a  high  degree  of  accuracy.  An  example  of  these  test  results  is  shown 
in  Figure  1.  The  mobile  node  used  for  this  experiment  is  a  Slocum  glider  with  a  dead¬ 
reckoning  system  as  its  only  navigation  method  available  during  submerged  operations. 


XV 


GPS  fixes  are  reeeived  at  the  beginning  and  end  of  the  traek,  and  dead-reekoning 
positions  are  estimated  when  submerged.  The  fixes  obtained  from  the  network  ranges 
show  a  significant  improvement  over  the  dead-reckoning  system. 
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Figure  1.  Slocum  UUV  track  from  July  21,  2005  showing  initial  and  final  GPS  and 
dead-reckoning  positions,  over-plotted  with  Seaweb  fixes 


I.  INTRODUCTION 


A.  MOTIVATION 

Future  U.S.  Navy  operations  are  expeeted  to  rely  heavily  on  unmanned  systems. 
For  operations  sueh  as  littoral  surveillanee  or  mine  elearing,  aceurate  navigation  of  these 
platforms  is  essential.  This  thesis  examines  the  use  of  an  aeoustie  network  designed  for 
eommunieating  with  such  platforms  as  the  basis  for  an  underwater  positioning  system 
similar  to  the  satellite  based  Global  Positioning  System.  An  algorithm  robust  enough  to 
derive  accurate  positions  from  range  measurements  is  needed  for  the  successful  operation 
of  this  system. 

B,  SEAWEB  OVERVIEW 

Seaweb  is  a  system  for  underwater  networked  acoustic  communications.  A 
Seaweb  network  consists  of  an  arbitrary  number  of  sensor  nodes,  repeater  nodes,  and 
gateway  nodes  as  described  in  [1].  Present  implementations  of  Seaweb  use  Benthos 
Telesonar  modems  that  are  readily  configurable  to  work  with  many  mobile  platforms 
including  submarines  and  UUVs. 

The  Seaweb  link-layer  protocol  includes  a  handshake  operation  from  which  the 
distance  between  the  two  communicating  modems  is  calculated  from  the  round-trip  travel 
time.  By  obtaining  simultaneous  ranges  between  a  mobile  node  and  a  number  of  fixed 
nodes,  a  Seaweb  network  can  double  as  a  portable  acoustic  tracking  grid  [2]. 
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c. 


SEAWEB  RANGING 

The  Seaweb  handshake  operation  is  illustrated  in  Figure  1. 


Figure  1. 


node  i  node  j 


The  round  trip  travel  time  is  given  by 

t  -t^  =  d.  +  T  +  d .  (l-l) 

where  dy  and  dji  are  the  transmission  times  from  node  i  to  node  j  and  from  node  j  to  node 
z,  respeetively,  and  z}  is  a  random  time  delay  inserted  by  the  responding  modem  to 
decrease  interference  with  other  communications.  Reciprocity  dictates  that  d..  =  d..  and 
the  one-way  travel  time  is  then 


t.-t.-T. 
J  0  J 


2 


(1.2) 


The  range  from  node  i  to  node  j  is  the  product  of  the  one-way  travel  time  and  the  speed  of 
sound  in  seawater.  The  speed  of  sound  is  assumed  to  be  a  nominal  value  of  1500m/s 
yielding  a  node-to-node  range  in  meters  of 


z;,  =1500 


tj-t^ 


\ 


2 


J 


(1.3) 
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The  ranging  calculations  are  performed  by  the  initiating  modem  and  ranges  are  logged  in 
a  database  by  the  Seaweb  server.  There  are  several  assumptions  in  this  ranging 
technique.  The  most  significant  assumptions  are  the  sound  speed  of  1500m/s  and 
straight-line,  direct  path  acoustic  propagation.  Despite  these  assumptions,  range  tests 
involving  stationary  nodes  have  shown  that  97%  of  the  ranges  are  accurate  to  within  10 
meters. 

In  the  broadcast  ping  process,  one  node  transmits  a  utility  packet  addressed  to  all 
listening  nodes  in  the  network.  The  receiving  nodes  each  respond  with  an  “echo”  utility 
packet  which  contain  the  data  necessary  to  calculate  the  node-to-node  range  in  equation 
(1.3).  During  the  experiments  analyzed  in  this  thesis,  broadcast  pings  from  the  UUVs 
were  initiated  by  a  command  from  the  tending  surface  vessel  sent  through  the  gateway 
node.  Range  data  were  calculated  from  the  echoes  by  the  modem  on  the  UUV  and  these 
data  were  transmitted  through  the  network  and  logged  on  the  Seaweb  server  on  the 
tending  vessel.  This  process  is  illustrated  in  Figure  2. 


Figure  2.  Broadcast  Ping.  The  operator  commands  the  UUV  (a)  to  broadcast  a  ping  (b). 

This  elicits  echoes  from  neighboring  nodes  (c).  The  UUV  telemeters  the 
calculated  set  of  ranges  back  to  the  operator  (d)  (from  [4]). 


D,  POSITION  FIXING  BY  RANGING 

In  an  n-dimensional  coordinate  system,  the  position  of  an  object  can  be 
determined  by  measuring  the  distance  between  the  object  and  n+\  known  points  as 
discussed  in  [3].  In  the  two-dimensional  case,  an  arc  is  drawn  centered  at  each  of  the 
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known  locations  with  a  radius  equal  to  the  respective  measured  range  as  graphed  in 
Figure  3.  The  position  fix  is  shown  by  the  intersection  of  the  three  ares.  Examples  of 
this  type  of  position  fixing  are  radar  navigation  and  the  Global  Positioning  System  (GPS). 
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Figure  3.  Two-dimensional  position  fix  by  ranging  method. 

Analytically,  if  the  position  of  the  zth  known  point  is  given  by  and  the  measured 
range  from  the  zth  point  to  the  objeet  being  fixed  is  given  by  r,,  the  distance  between  the 
known  point  and  the  position  of  the  object  (x,y)  is  given  by  the  Pythagorean  relationship. 

r^  =  {x-xf+{y-yf  (1.4) 

Sinee  equation  (1.4)  is  quadratie  in  two-dimensions,  the  position  fix  (x,y)  is  the  solution 
to  a  system  of  zz+l  simultaneous  Pythagorean  equations 
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(x-x^  +(y-y^f 

^2 

= 

{x-x^f  +{y-y^f 

_(x  -  x^f  +  (y  -  y^f  _ 

X^  -  2xXj  4-  X^  +  y^  - 

= 

x^  -  Ixx^  +x^  +  y^  - 

-  2xx^  +  X3  +  _y^  - 


2 

2  +  3^2 

2  +  y' 


(1.5) 


Equation  (1.5)  is  referred  to  as  the  fix  equation  and  the  unknown  vector  [x  yY  as  the 
position  fix. 

In  Figure  3  the  ranges  to  the  known  points  are  assumed  to  be  error  free  and  there 
is  a  precise  solution  for  the  position  fix.  If  the  ranges  do  contain  errors  then  a  precise 
solution  is  not  obtainable,  however  an  area  with  high  probability  of  containing  the  true 
position  is  indicated  by  the  region  surrounded  by  the  intersections  of  the  range  circles  as 
shown  in  Figure  4. 
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Figure  4.  Two-dimensional  position  fix  with  imperfect  range  data. 
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II.  BACKGROUND  ON  UNDERWATER  POSITIONING 


Previous  use  of  Seaweb  as  a  positioning  system  was  based  on  solving  for  the 
interseetions  of  pairs  of  range  cireles.  This  is  equivalent  to  solving  only  the  top  two  rows 
in  equation  (1.5).  Beeause  of  the  quadratie  nature  of  the  range  eirele  equations,  these 
pair-wise  algorithms  result  in  two  solutions,  one  of  whieh  corresponds  to  the  true  position 
fix  while  the  other  corresponds  to  a  reflection  across  the  line  connecting  the  two  known 
points  as  shown  in  Figure  5. 
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Figure  5.  Ambiguous  fix  resulting  from  two  range  measurements 


For  A  fixed  nodes,  this  yields  a  total  of  5  =  2 


v2. 


solutions  since  each  pair  of  fixed 


nodes  will  in  general  yield  two  intersections. 
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Two  algorithms  are  implemented  whieh  eompare  these  solutions  to  eaeh  other  to 
eliminate  the  ambiguous  estimates.  Both  of  the  methods  are  based  on  the  premise  that 
the  intersections  that  correspond  to  the  correct  position  will  be  concentrated  around  the 
true  position  while  the  ambiguous  solutions  will  be  significantly  separated  from  this 
cluster. 

A.  WEIGHTING  METHOD 

In  2005  [2]  created  an  algorithm  in  which  a  weighting  value  is  assigned  to  each 
possible  solution.  The  weighting  value  Wi  for  a  particular  solution  Xi  is  based  on  the 
proximity  of  the  solution  to  all  other  calculated  solutions.  Those  solutions  corresponding 
to  the  true  position  are  assigned  a  higher  weighting  value.  The  ambiguous  solutions  are 
separated  both  from  the  true  solution  cluster  and  from  the  other  ambiguous  solutions  and 
receive  a  low  weighting  factor.  The  final  position  X  is  then  a  weighted  average  given  by 


^  s 


X  = 


V  i=l 


(2.1) 


B,  CENTER  OF  MASS  METHOD 

Also  in  2005,  [4]  tested  an  algorithm  that  attempts  to  eliminate  the  ambiguous 
solution  by  comparing  both  solutions  from  a  given  node  pair  to  the  overall  mean  position 
from  all  possible  node  pairs.  For  each  fixed  node  pair,  only  the  solution  closest  to  the 
mean  position  is  used  to  compute  the  final  position. 

C.  DIFFERENCE  LINEARIZATION  METHOD 

In  [5],  Krause  formulates  an  algebraic  solution  to  the  GPS  equations  by  a  method 
called  difference  linearization  in  which  the  equation  corresponding  to  a  given  satellite  is 
subtracted  from  another  equation  in  the  system.  This  results  in  a  set  of  linear  equations 
that  can  be  solved  for  the  receiver  position  and  time  offset  without  the  need  for  an 
iterative  method.  This  thesis  implements  a  similar  method  and  compares  performance  to 
the  pairwise  weighting  methods  previously  implemented. 
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III.  DIFFERENCE  LINEARIZATION  METHOD 


A.  FORMULATION 

As  stated  in  Chapter  II,  previous  work  by  Hahn  and  Ouimet  solved  the  equations 
for  pairs  of  interseeting  range  eireles.  This  thesis  evaluates  the  use  of  the  differenee 
linearization  algorithm  in  [5]  for  solving  the  fix  equation.  This  algorithm  solves  a 
simultaneous  set  of  three  range  eirele  equations  as  given  in  equation  (1.5)  by 
reformulating  the  equality  sueh  that  the  squared  unknown  terms  are  eliminated.  This 
leads  to  a  pair  of  simultaneous  linear  equations  in  x  and  y 


1 

>  K> 

1 

1 

=  2 

1 - 

1 

(N 

1 

<N 

1 _ _ _ _ 

X 

+ 

{xl  -xl)  +  {yl  -yl) 

\S^2 

[(X3-X2)  {y,-y^)\ 

Lt_ 

_(xl-xl)  +  {yl-yl)_ 

with  a  solution  of 


X 

1 

(Xj-Xi)  (Jj-Ti) 

-1 

\r^^-r^^)Hxl-xl)  +  {yl-yl) 

_yl 

^2 

_(X3-X2)  iy,-y,)\ 

_{rl  -rl)+  {xl  -xl)  +  {yl  -  y]) _ 

For  simplicity  of  notation,  letx  = 


LT. 

(r^  -r^)  +  {xl  -xl)  +  {yl  -  y^ ) 


(x^-Xi)  (y^ 

(X3-X2)  (^3 


Ti) 

T2) 


,  and 


so  that  equation  (3.2)  can  be  written  as 


X  = 


(3.3) 


This  method  of  difference  linearization  can  be  extended  to  an  overdetermined 
system.  In  the  case  of  a  network  with  N  fixed  nodes  the  matrix  F  then  has  size  (A-1  x  2) 
and  the  vector  a  has  (N-1)  elements.  An  overdetermined  method  of  positioning  was 
implemented  as  part  of  this  thesis,  but  it  was  found  to  be  less  accurate  than  taking  all 
available  combinations  of  three  nodes  and  averaging  the  solutions.  One  significant  range 
error  would  skew  the  solution  even  if  the  other  ranges  were  reliable. 
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In  the  combination  method,  individual  solutions  could  be  checked  for  accuracy 
and  discarded  if  they  did  not  pass.  The  combination  algorithm  also  maintains  P  as  a 
square  matrix  allowing  exact  solution  by  matrix  inversion,  in  contrast  to  a  least-squares 
estimation. 

B,  SOURCES  OF  RANGE  ERRORS 

There  are  several  sources  of  error  that  can  degrade  the  accuracy  of  the  measured 
ranges,  and  hence  lead  to  errors  in  the  calculated  positions.  If  a  measured  range  r.  is 

corrupted  by  measurement  error  Ar,  then  the  measured  range  is  r.  =r.+  Ar.  and  vector  a 
in  equation  (3.3)  is  estimated  as 

(ri"  +  Ir^Ar^  +  Ar^  -  -  Ir^Ar^  -  Ar/ )  +  (x^  -  xf  )  +  (y^  -  yf  ) 

(r/  +  Ir^Ar^  +  Ar/  -  r/  -  Ir^^Ar^  -  Ar/ )  +  (Xj  -  Xj )  +  (y^  -  y^) 


(3.4) 


Equation  (3.3)  will  still  have  a  mathematical  solution  when  a  is  replaced  with  a  ,  but  this 
solution  will  not  correspond  to  the  true  position  when  measurement  errors  become 
significant. 

Because  the  range  errors  for  each  fixed  node  are  independent  and  their  effects  on 
the  position  fix  are  nonlinear,  it  is  difficult  to  perform  a  sensitivity  analysis  of  a  given 
positioning  algorithm.  In  the  simple  case  of  only  one  range  containing  errors,  say  ri,  it 
can  be  shown  that  the  position  errors  will  be  given  by 


,^._(T3-T2)(2^A+V) 

ZAA-  — 

2det(E) 

^  _(x,-X3)(2rjAr,+Ar/) 
2det(E) 


(3.5) 


which  shows  that  the  position  errors  are  also  dependent  on  the  fixed  node  geometry. 
Similar  equations  can  be  developed  for  errors  in  r2  or  rs.  The  positioning  algorithm  uses 
range  data  from  all  available  nodes  such  that  if  only  a  small  portion  of  the  available 
ranges  are  affected  by  significant  range  error,  an  accurate  position  can  still  be  calculated 
from  the  uncorrupted  ranges.  Several  case  studies  are  presented  in  Chapters  V  and  VI  in 
support  of  a  range  error  sensitivity  analysis. 
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Another  method  of  characterizing  the  position  error  is  to  determine  the  covariance 
of  the  position  estimate  x  given  by 

Cov(x)  =  EU}J-^^E{^r^r4  =  MTI  Cov(A,)f  (3.6) 

^  ’  dr_  ^  ’\dr_)  dr_  \  ) 

where  r  is  the  vector  of  ranges  and  /(r)  is  defined  in  (3.2)  so  that 


dx  dx  dx 

df{r_)  ^  Qr,  dr^  dr^ 

dy  dy  dy 

dr^  dr^  dr^ 


1  ''1(3^3- 3^2)  ''2(3^1 -3^3)  ''3(3^2 -3^1) 

det(P)  [rj(x2  - X3)  r^{x^  - x^)  r^{x^  - x^) 


An  estimate  of  this  covariance  matrix  is  required  if  the  Seaweb  fixes  are  to  be  integrated 
into  an  onboard  navigation  system  with  a  Kalman  filter.  For  simplicity  we  assume  that 
the  errors  in  the  range  measurements  are  independent  of  one  another  but  have  the  same 
standard  deviation  which  gives 


Cov(  Ar)  =  (j  I 


Even  for  this  simple  case,  the  partial  derivative  matrix  given  by  equation  (3.7)  shows  that 
the  covariance  of  the  position  error  is  dependent  on  both  the  network  layout  and  the  range 
to  the  vehicle.  In  reality  there  may  be  environmental  or  system  features  which  result  in 
different  standard  deviations  for  each  node  or  which  might  introduce  coupling  in  the 
range  errors  from  different  nodes  that  would  lead  to  non-zero  terms  in  the  off-diagonal 
elements. 

1,  Neglecting  Depth 

While  the  difference  linearization  algorithm  can  easily  be  extended  to  three 
dimensions,  the  errors  present  in  the  ranges  can  cause  significant  errors  in  the  solution  of 
the  depth  coordinate.  This  is  due  mainly  to  geometric  dilution  of  precision  effects 
(discussed  in  section  C  of  this  chapter)  as  the  fixed  nodes  are  very  nearly  coplanar  in  the 
z-dimension.  Even  with  minor  variations  in  bathymetry  over  the  network  layout,  the 
variation  of  fixed  node  depth  is  much  smaller  than  the  spread  in  x  and  y.  Because  of  this, 
we  have  chosen  to  neglect  depth  and  solve  only  for  the  two-dimensional  position. 
Ignoring  depth  obviously  causes  some  error  in  the  range  measurements  because  the 
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vehicle  will  not  be  in  the  same  plane  as  the  fixed  nodes.  The  closer  the  vehicle  (in  x  and 
y)  to  a  given  fixed  node,  the  more  the  range  to  that  node  will  reflect  the  difference  in 
depth  between  the  vehicle  and  the  node.  A  simple  illustration  is  provided  in  Figure  6.  If 
the  angle  0  is  less  than  71.8°,  then  the  range  error  between  r  and  ru  caused  by  neglecting 
depth  will  exceed  5%.  For  a  depth  separation  z  between  the  vehicle  and  node  of  100m, 
this  corresponds  to  a  minimum  good  range  of  approximately  350m.  In  most  cases, 
however,  if  the  vehicle  is  very  close  to  one  fixed  node  such  that  the  range  is  affected  by 
depth,  it  will  be  far  enough  away  from  the  other  nodes  in  the  network  to  minimize  this 
effect  for  the  remaining  nodes. 


Figure  6.  Effect  of  depth  on  vehicle  range 

Future  network  implementations  can  eliminate  this  source  of  range  error  by  including 
depth  sensors  at  all  nodes. 

2,  Speed  of  Sound  and  Refraction 

If  the  speed  of  sound  estimate  of  1500m/s  used  in  equation  (1.3)  is  not  accurate, 
the  calculated  ranges  will  have  corresponding  inaccuracies.  Sound  speeds  in  the  ocean 
typically  range  from  1480  to  1520  m/s  or  1.3%  from  the  assumed  value  [6].  Ranges 
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calculated  with  an  incorrect  sound  speed  will  have  the  same  percentage  error  as  the 
mismatch  in  sound  speed.  Also,  sound  speed  often  varies  with  depth,  whieh  leads  to  a 
refraetive  ray-path.  The  length  of  this  refraetive  path  will  be  longer  than  a  straight-line 
path  between  the  eommunieating  nodes. 

3.  Multipath 

Multipath  error  ean  arise  if  the  channel  geometry  and  sound  speed  profile  allow 
for  multiple  ray-paths  such  as  a  surface  or  bottom  reflection  in  addition  to  the  direct  path. 
The  direct  path  is  usually  the  shortest  range  and  the  most  accurate.  In  most  geometries  of 
interest  it  should  also  have  the  highest  SNR  of  the  several  path  lengths.  While  the 
modems  utilize  a  peak-deteetor  filter  whieh  should  eliminate  most  multipath  effects,  it  is 
possible  that  a  multipath  response  may  aetually  have  a  higher  SNR  than  the  direet  path 
and  thus  be  inadvertently  used  for  the  range  ealculation. 

4.  Vehicle  Motion 

A  vehicle  in  motion  will  travel  some  distance  between  the  time  it  issues  the 
broadcast  ping  request  and  the  time  it  receives  the  response.  The  worst  case  for  this  type 
of  error  occurs  when  the  vehicle  is  moving  directly  towards  or  away  from  a  node.  For 
example,  a  5kt  vehicle  1500m  away  from  a  fixed  node  would  have  a  motion  induced 
error  of  2.57m  or  0.17%  [3].  Also,  the  responses  are  not  normally  received  precisely 
simultaneously.  Because  of  this,  the  vehicle  will  be  at  a  slightly  different  position  at  the 
receipt  of  each  ping  response.  This  effect  should  be  minimal  if  the  responses  are  received 
within  a  few  seconds  of  each  other. 

C.  GEOMETRIC  DILUTION  OF  PRECISION 

The  error  in  the  calculated  position  is  dependent  on  both  the  errors  in  the  range 
measurements  and  the  geometry  of  the  fixed  nodes.  The  influence  of  the  fixed  node 
geometry  is  a  phenomenon  known  as  geometric  dilution  of  precision  (GDOP).  If  each 
range  arc  is  represented  as  an  annulus  to  model  the  uncertainty  in  the  measured  range,  the 
intersection  of  these  annuli  is  then  an  area  of  likely  target  position.  If  the  angle  Otr  is 
defined  with  vertex  at  the  intersection  of  the  annuli  and  rays  to  the  center  of  each 
annulus,  then  as  Otr  becomes  very  small,  the  GDOP  effects  become  more  pronounced. 
The  effects  of  GDOP  are  minimized  when  Otr  is  90°.  This  is  illustrated  in  Figure  7. 
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Figure  7.  Geometric  Dilution  of  Precision  (from  [7]) 


When  using  range  data  from  three  nodes  the  limiting  worst  case  of  GDOP  occurs  when 
all  of  the  fixed  nodes  are  collinear.  In  this  case  it  is  impossible  to  determine  which  side 
of  the  line  corresponds  to  the  true  position.  Analytically  this  is  because  the  matrix  P 


from  equation  (3.3)  given  by 


is  singular  for  the  collinear  case. 


(x^-x^)  (y,-y,)_ 

Since  the  GDOP  characteristics  of  the  network  are  only  dependent  on  the  geometry  of  the 
fixed  nodes,  regions  of  high  GDOP  can  be  minimized  during  the  network  layout  phase. 
A  measure  of  the  GDOP  characteristics  can  be  found  by  calculating  the  condition  number 
of  the  P  matrix  [8].  A  condition  number  near  one  indicates  good  GDOP  characteristics 
while  a  large  condition  number  indicates  a  nearly  singular  matrix  and,  as  a  result,  poor 
GDOP  characteristics. 
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IV.  ALGORITHM  IMPLEMENTATION 


A.  PROGRAM  ORGANIZATION 

The  position  fixing  methods  are  implemented  in  Matlab.  The  program  is  modular 
for  readability,  maintainability,  and  applieability  to  both  simulated  and  aetual  measured 
data. 


1.  Input  and  Data  Acquisition 

Input  meehanisms  are  provided  for  reading  fixed  node  positions  (latitude  and 
longitude)  in  either  deeimal  degrees  or  degrees,  minutes,  and  seeonds.  Ping  ranges  to 
these  fixed  nodes  are  also  read  from  either  text  or  Exeel  files.  Inputting  the  data  outside 
the  positioning  funetion  allows  for  readier  use  with  other  fixing  algorithms  without 
reloading  the  data  files. 

2.  Transformation  from  Latitude  and  Longitude  to  x-y 

For  loeating  the  fixed  nodes  and  eomparison  to  other  positioning  systems  sueh  as 
GPS  or  onboard  inertial  systems,  eoordinates  in  latitude  and  longitude  are  transformed 
into  a  loeal  eoordinate  system.  Sinee  the  length  of  both  a  degree  of  latitude  and  a  degree 
of  longitude  vary  due  to  the  shape  of  the  Earth,  these  distanees  are  speeifieally 
determined  for  the  operating  area  aeeording  to  equations  provided  by  [9].  Onee  these 
lengths  are  loeally  determined  for  the  operating  latitude,  a  point  such  as  one  of  the  fixed 
nodes  is  chosen  as  the  origin  of  the  local  coordinate  system.  The  latitude  and  longitude 
of  each  point  of  interest  are  subtracted  from  those  of  the  origin  point  and  a  simple  unit 
conversion  based  on  the  calculated  degree  lengths  is  performed  to  obtain  coordinates  in 
meters  relative  to  the  chosen  origin. 

3.  Position  Determination 

Since  this  algorithm  solves  the  set  of  three  simultaneous  equations  given  by 


equation  (1.5),  there  are  actually 


v3. 


positions  calculated  for  each  broadcast  ping. 


where  N  is  the  number  of  fixed  nodes.  The  program  does  not  assume  a  specific  number 


of  fixed  nodes,  so  the  value  of 


v3y 


is  calculated  at  the  start  of  the  program  as  a  loop 


control  parameter.  The  combinations  themselves  are  also  generated  according  to  the 
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algorithm  in  [10]  for  use  as  subscripts  that  index  the  specific  node  positions  and  ranges 
for  each  instance  of  calculation.  With  a  given  set  of  node  positions  and  ranges  as  inputs, 
the  matrix  and  vector  given  in  equation  (3.2)  are  formed  and  the  equation  solved  for 

[x  .  The  position  is  then  verified  by  comparing  back-calculated  ranges  from  the 

calculated  position  to  the  input  ranges.  If  these  back-calculated  ranges  differ  from  the 
input  ranges  by  more  than  10%  of  the  input  ranges,  the  solution  for  that  set  of  nodes  is 
considered  invalid  and  ignored.  When  a  solution  has  been  calculated  for  each 
combination  of  fixed  nodes,  the  valid  solutions  are  averaged  to  determine  a  final  position 
for  that  time  instance.  In  addition  to  calculating  a  solution  for  each  combination  of  fixed 
nodes,  a  solution  of  the  overdetermined  difference  linearization  equation  described  in 
Chapter  III  is  also  calculated  in  order  to  compare  the  relative  accuracy  of  these  two 
methods.  For  further  comparison,  the  weighting  method  and  center  of  mass  method  are 
calculated. 

4.  Output 

The  output  of  the  program  is  normally  a  chartlet  that  shows  the  position  of  each 
fixed  node  and  the  final  positions  calculated  for  each  time  instance.  These  plots  are 
performed  in  a  different  function  so  that  tracks  from  other  position  sources  or  calculated 
by  other  algorithms  can  be  overplotted  for  comparison. 
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V.  ALGORITHM  TESTING  WITH  SYNTHETIC  DATA 


A,  ASSUMPTIONS  AND  SETUP 

In  July  2005  ranging  experiments  were  eonducted  in  Monterey  Bay  using  a 
Sloeum  glider  UUV  as  a  mobile  node  within  a  Seaweb  network  [4],  The  fixed  grid 
geometry  in  these  experiments  is  used  as  the  grid  in  the  following  simulations.  The 
reeorded  GPS  positions  and  the  assoeiated  x-y  positions  are  shown  in  Table  1  and  a  chart 
of  the  node  positions  is  given  in  Figure  8. 


Latitude 

Longitude 

y  Position 
Difference 

X  Position 
Difference 

Node 

(Decimal 

Degrees) 

(Decimal 

Degrees) 

(meters) 

(meters) 

R10 

36.70563 

-121.96362 

0.00 

0.00 

R11 

36.71463 

-121.96367 

998.75 

-4.47 

R12 

36.70837 

-121.95308 

304.06 

941.79 

R13 

36.6984 

-121.95702 

-802.33 

589.73 

R14 

36.69864 

-121.96871 

-775.69 

-454.81 

R15 

36.70852 

-121.97417 

320.71 

-942.68 

Table  1 .  Fixed  node  positions  for  simulations  and  July  2005  Slocum  test  showing  original 
latitude/longitude  and  positions  in  meters  relative  to  center  node 


Figure  8. 


July  2005  grid  geometry 
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The  condition  number  of  the  P  matrix  for  each  combination  of  three  nodes  is  shown  in 
Table  2.  As  most  of  the  condition  numbers  are  between  1.5  and  2.1,  this  network  shows 
good  GDOP  performance.  However  there  are  some  sets  such  as  nodes  RIO,  Rll,  and 
R14  that  give  weaker  GDOP  performance.  Detrimental  effects  from  poor  GDOP 
performance  are  minimized  by  discarding  those  solutions  that  fail  to  satisfy  the  original 
range  equations  within  the  10%  criteria  discussed  in  Chapter  IV. 


Nodes 

Condition  Number 
of  P  matrix 

RIO,  R11,R12 

2.0290 

RIO,  R11,R13 

7.7187 

RIO,  R11,R14 

9.3952 

RIO,  R11,R15 

1.9820 

RIO,  R12,  R13 

1.9859 

RIO,  R12,  R14 

6.7679 

RIO,  R12,  R15 

7.5646 

RIO,  R13,R14 

2.0441 

RIO,  R13,R15 

7.9865 

RIO,  R14,  R15 

2.0839 

R11,R12,  R13 

1.3928 

R11,R12,  R14 

1.6504 

R11,R12,  R15 

3.5279 

R11,R13,R14 

2.0178 

R11,R13  R15 

3.1235 

R11,R14,  R15 

3.2134 

R12,  R13,R14 

1.3576 

R12,  R13,R15 

1.8224 

R12,  R14,  R15 

1.5818 

R13,R14,  R15 

1.6182 

Table  2.  P  matrix  condition  numbers  for  Monterey  Bay  Seaweb  Grid 


1.  Data  Characteristics 

A  mesh  with  spacing  every  200m  from  (x,y)  =  (-1500,  -1500)  to  (1500,  1500) 
resulted  in  256  test  points.  Error-free  ranges  from  each  of  the  six  nodes  to  each  of  these 
256  points  were  calculated.  The  error  free  ranges  were  then  corrupted  with  simulated 
measurement  noise  according  to 

r.=r.+Ar.  (5.1) 


18 


where  Ar,  is  Gaussian  noise  with  mean  zero  and  standard  deviation  (in  meters).  These 
eorrupted  ranges  were  then  used  as  the  input  to  the  four  different  position  fix  algorithms 
(with  minor  ehanges  to  the  programs  from  [2]  and  [4]  to  use  the  same  ranges  in  all  four 
methods).  The  caleulated  fix  positions  were  compared  to  the  true  positions 

(x.,  j.)  of  the  256  point  mesh  with  the  position  error  in  meters  given  by 


(x.-x.f +(>'.-y.)' 


nl/2 


(5.2) 


The  entire  mesh  was  tested  in  this  manner  1000  times  for  each  value  of  (Jr  so  that  the 
resultant  errors  from  each  of  the  four  algorithms  could  be  compared. 

B,  CASE  STUDIES 

1,  Data  Set  with  Zero  Range  Error 

To  ensure  the  validity  of  all  of  the  position  fix  algorithms  a  data  set  with  zero 
range  error  was  used  as  an  initial  case.  All  of  the  algorithms  performed  well  in  this  case, 
with  no  errors  greater  then  0.5m.  Even  for  this  simple  case,  the  center  of  mass  method 
has  significantly  larger  errors  than  the  other  methods.  This  indicates  that  for  certain  pairs 
of  range  circles  the  center  of  mass  method  is  choosing  the  wrong  solution.  The  mean  and 
maximum  errors  from  each  algorithm  are  presented  in  Table  3. 


Combination 

method 

Overdetermined 

method 

Center  of  Mass 
Method 

Weighting 

Method 

mean  error  (m) 

0.0039 

0.0045 

0.0126 

0.0035 

max.  error  (m) 

0.0103 

0.0134 

0.4716 

0.0167 

Table  3.  Position  error  statistics  for  zero  range  error  data 


2.  Data  Sets  with  Gaussian  Range  Error 

Several  case  studies  were  performed  for  the  case  of  Gaussian  range  error.  The 
standard  deviation  values  used  were  o>=4m,  15m,  30m,  50m,  and  75m.  Histograms  of 
the  position  errors  from  each  algorithm  and  each  <Jr  value  are  shown  in  Figures  9-13.  As 
expected,  the  magnitude  of  the  position  errors  increases  with  increasing  range  error,  but 
from  the  histograms  it  can  be  seen  that  the  maximum  error  from  the  two  difference 
linearization  methods  is  much  less  than  that  from  the  pairwise  methods.  The  mean, 
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standard  deviation,  and  maximum  errors,  as  well  as  confidence  interval  values  for  50%, 
75%,  90%,  95%,  and  99%  are  given  in  tables  4-8  following  each  group  of  histograms. 
The  statistics  presented  in  the  tables  also  validate  the  use  of  the  difference  linearization 
algorithm.  In  all  of  the  tested  cases,  the  mean,  standard  deviation,  and  maximum  errors 
are  smallest  with  the  combination  method  of  the  difference  linearization  algorithm. 
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Figure  9.  Position  error  histograms  for  cr, .=4m 
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Method 
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5.2 

6.0 

11.0 

5.5 

Error  Std. 
Dev. 

3.0 

3.7 

12.3 

4.0 

Max.  Error 

26.3 

35.3 

198.8 

75.3 

50%  Error 

4.7 

5.4 

7.2 

4.7 

75%  Error 

6.9 

8.0 

13.0 

7.1 

90%  Error 

9.3 

11.0 

23.3 

10.2 

95%  Error 

10.8 

13.0 

33.2 

12.8 

99%  Error 

14.1 

17.3 

64.8 

19.9 

Table  4.  Position  error  statistics  for  cr, .=4m,  all  values  in  meters 
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Figure  10.  Position  error  histograms  for  o>=15m 
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15.1 
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130.3 
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17.7 
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25.5 

17.5 

75%  Error 

25.9 
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44.1 

26.5 

90%  Error 

34.7 

41.0 

74.0 

38.2 

95%  Error 

40.6 

48.5 

99.4 

48.0 

99%  Error 

52.7 

64.8 

164.9 

75.2 

Table  5.  Position  error  statistics  cr, ~1 5m,  all  values  in  meters 
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Figure  1 1 .  Position  error  histograms  for  o>=30m 
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155.9 

Table  6.  Position  error  statistics  cr, “30m,  all  values  in  meters 
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Figure  12.  Position  error  histograms  for  o>=50m 
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71.3 

Error  Std. 
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56.6 

Max.  Error 
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1603.3 

50%  Error 

59.1 
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75%  Error 

86.6 
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89.7 

90%  Error 
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137.1 

209.7 
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95%  Error 

135.8 

162.3 

269.6 

166.6 

99%  Error 

176.9 

216.2 

409.9 

275.2 

Table  7.  Position  error  statistics  for  cr, =5  Om,  all  values  in  meters 
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Figure  13.  Position  error  histograms  for  cr, =75m 
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Dev. 
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68.3 

118.6 

94.9 

Max.  Error 

489.9 

638.1 

1958.2 

2874.9 

50%  Error 

88.5 

100.7 

116.6 

89.1 

75%  Error 

129.7 

150.0 

191.0 

135.9 

90%  Error 

173.9 

205.1 

298.6 

200.8 

95%  Error 

203.3 

243.0 

382.4 

261.3 

99%  Error 

264.4 

324.7 

575.5 

470.9 

Table  8.  Position  error  statistics  cr,  =75m,  all  values  in  meters 


3,  Data  Set  with  Only  Positive  Range  Error 

A  data  set  was  also  tested  using  Ar,  equal  to  the  absolute  value  of  a  Gaussian 
distribution  with  cr,-25m.  The  absolute  value  was  used  because  most  of  the  error  sources 
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discussed  in  Chapter  III  result  in  an  overestimate  of  the  range,  the  exception  being 
underestimation  of  sound  speed.  Again  the  combination  method  of  the  difference 
linearization  algorithm  showed  the  best  performance  of  the  methods  tested.  Histograms 
of  the  error  distribution  are  shown  in  Figure  14  with  statistics  in  Table  9. 
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Figure  14.  Position  error  histograms  for  positive  Gaussian  error,  cr, “25m 
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Weighting 

Method 

Mean  Error 

23.5 

26.9 

41.3 

28.5 

Error  Std. 
Dev. 

14.0 

17.5 

32.6 

17.1 

Max.  Error 

123.6 

200.9 

454.5 

360.9 

50%  Error 

21.0 

23.3 

32.9 

25.7 

75%  Error 

31.7 

36.0 

51.0 

36.7 

90%  Error 

42.8 

50.3 

78.7 

49.3 

95%  Error 

50.0 

60.4 

103.2 

58.8 

99%  Error 

64.3 

82.4 

168.1 

84.1 

Table  9.  Position  error  statistics  for  positive  Gaussian  error,  o;-=25m,  all  values  in  meters 
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C.  TESTING  RESULTS 

Throughout  the  range  of  values  tested,  the  combination  difference  linearization 
method  consistently  performed  the  best  of  all  four  methods.  Also,  for  the  case  of 
Gaussian  range  errors,  both  the  mean  and  standard  deviation  Gp  of  the  position  error  were 
found  to  be  linear  functions  of  cr,-  as  shown  in  Figure  15.  The  slope  of  the  line  for  mean 
error  vs  is  1.3  and  the  slope  of  the  line  for  cy  vs  is  0.75.  Both  lines  have  a  zero 
intercept.  This  result  allows  prediction  of  the  position  error  if  the  distribution  of  input 
error  is  known. 
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Figure  15.  Mean  and  standard  deviation  of  position  error  as  a  function  of  cr,- 
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VI.  SYNTHETIC  VEHICLE  TRACKS 


A,  ASSUMPTIONS  AND  SETUP 

Two  simple  tracks  were  simulated  and  tested  in  a  similar  manner  to  the  mesh  of 
known  points  tested  in  Chapter  V.  The  first  track  consisted  of  a  single  leg  from 
(-1200,  700)  to  (900,  -1100).  Fifty-one  equally  spaced  points  were  calculated  along  the 
one  leg  defined  by  these  waypoints.  The  second  track  had  two  legs  defined  by  the 
waypoints  (-1200,  700),  (-900  -500),  and  (900,  -1100).  There  were  101  equally  spaced 
points  along  the  second  track,  50  per  leg  plus  the  final  waypoint.  The  tracks  are  shown  in 
Figure  16.  Error- free  ranges  from  each  fixed  node  to  each  of  the  track  points  were 
calculated  and  corrupted  with  Gaussian  noise  for  the  simulations.  As  in  Chapter  V,  1000 
iterations  were  performed  for  each  noise  value. 
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Figure  16.  Synthetic  tracks  1  and  2 

B.  CASE  STUDIES 

Two  values  of  Gaussian  error  were  added  to  the  ranges  for  each  track.  The  values 
of  (Jr  for  these  case  studies  were  4m  and  50m.  Position  error  histograms  are  shown  in 
Figures  17-19  with  error  statistics  in  accompanying  Tables  10-13.  In  all  of  the  cases 
presented,  the  combination  implementation  of  the  difference  linearization  algorithm 
provides  the  best  performance  of  the  four  methods. 


27 


2 


Combination  Method 


Overdetermined  Method 


(fi 

03 

O 

c 

03 


D 

O 

O 

o 

o 


(Ji 

03 

O 

c 

^3 

O 

o 

o 

o 


2  4  6  8  10  >10 


position  error,  meters 


2  4  6  8  10  >10 


position  error,  meters 


w 

03 

o 

c 

03 


O 

o 

o 

o 


(fi 

03 

O 

c 

I 

D 

o 

o 

o 

o 


2 

1.5 

1 

0.5 

0 

2 

1.5 

1 

0.5 

0 

2  4  6  8  10  >10 

position  error,  meters 


2  4  6  8  10  >10 

position  error,  meters 

Weighting  Method 


Figure  17.  Position  error  histograms  for  single  leg  traek  with  o;~4m 
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Table  10.  Position  error  statistics  for  single  leg  track  with  cr, -=4m,  all  values  in  meters 
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Figure  18.  Position  error  histograms  for  single  leg  track  with  cr, -=50m 
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Table  1 1 .  Position  error  statistics  for  single  leg  track  with  0=5  Om,  all  values  in  meters 


29 


Combination  Method 
15^ 


X 


2  4  6  8  10  >10 

position  error,  meters 


2  4  6  8  10  >10 

position  error,  meters 


Overdetermined  Method 

15^ - - - - 

"b 


X 


2  4  6  8  10  >10 

position  error,  meters 
Weighting  Method 

15 1 - - - - - - ^ 

"b 


X 


2  4  6  8  10  >10 

position  error,  meters 


Figure  19. 


Position  error  histograms  for  two  leg  track  with  o>=4m 
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Table  12.  Position  error  statistics  for  two  leg  track  with  o>=4m,  all  values  in  meters 
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Figure  20. 


Position  error  histograms  for  two  leg  track  with  cr, . =5  Om 
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Table  13.  Position  error  statistics  for  two  leg  track  with  cr, =5 Om,  all  values  in  meters 
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C.  TESTING  RESULTS 

Although  the  statistical  properties  of  the  range  values  were  the  same,  the  mean 
and  error  covariance  of  the  position  errors  for  each  set  of  cases  were  consistently  smaller 
than  those  from  similar  tests  in  Chapter  V.  This  is  most  likely  due  to  the  fact  that  the 
tracks  do  not  cover  the  entire  possible  range  of  GDOP  effects. 
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VII.  AT  SEA  TESTING 


A,  JULY  2005  SEAWEB  TESTING  IN  MONTEREY  BAY 

The  July  2005  Seaweb  testing  used  a  Sloeum  glider  UUV  as  a  mobile  node  to  be 
tracked  within  the  fixed  grid  [4],  This  vehicle  is  buoyancy  driven  and  as  such  has 
significant  depth  excursions  during  operation  as  illustrated  in  Figure  21. 
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Figure  2 1 .  Slocum  sawtooth  motion  (after  [11]) 

A  representative  track  from  the  Slocum  experiment  is  shown  in  Figure  22.  This 
figure  shows  the  potential  for  navigation  improvement  obtained  by  utilizing  Seaweb 
position  fixes.  Since  the  GPS,  dead-reckoning,  and  Seaweb  positions  are  not  recorded  for 
precisely  the  same  times,  an  exact  comparison  is  not  possible,  but  it  is  apparent  from  the 
figure  that  the  dead-reckoning  positions  did  not  match  the  GPS  fix  obtained  by  the 
vehicle  when  it  returned  to  the  surface.  The  final  dead-reckoning  position  is  227.1m 
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from  the  final  GPS  fix.  The  Seaweb  fixes,  however,  appear  to  track  well  with  the  true 
vehicle  motion,  with  the  final  Seaweb  fix  only  66.4m  from  the  GPS  endpoint.  From  the 
Seaweb  fixes,  it  appears  that  the  vehicle  was  attempting  to  follow  the  dead-reckoning 
track  until  it  was  pushed  west  by  currents. 
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Figure  22.  Slocum  UUV  track  from  July  21,  2005  showing  initial  and  final  GPS  and 

dead-reckoning  positions,  over-plotted  with  Seaweb  fixes 


The  ranging  data  used  for  positioning  the  Slocum  are  given  in  Table  14.  During 
this  event,  21  broadcast  ping  requests  were  issued  with  a  possibility  of  126  responses 
from  the  fixed  grid  and  420  positioning  combinations  for  the  track.  Only  80  responses 
were  actually  received.  The  responses  are  grouped  in  such  a  way  that  there  were  99 
possible  positioning  combinations.  Valid  solutions  are  obtained  from  59  of  these  99 
combinations.  The  remaining  40  combinations  yield  solutions  that  do  not  satisfy  the  10% 
agreement  criterion  with  the  measured  ranges. 
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Time 

RIO 

Rll 

R12 

R13 

R14 

R15 

1119:36 

579.6 

827.5 

1423.9 

1563.0 

NaN 

503.1 

1121:35 

544.9 

NaN 

1416.3 

NaN 

NaN 

501.3 

1123:41 

523.6 

895.2 

1413.4 

1484.4 

NaN 

510.1 

1125:47 

518.7 

NaN 

1409.4 

NaN 

NaN 

NaN 

1128:35 

504.6 

NaN 

1414.5 

1420.2 

979.5 

NaN 

1132:05 

437.4 

NaN 

NaN 

1331.4 

882.1 

607.5 

1137:34 

NaN 

NaN 

1415.8 

NaN 

784.0 

NaN 

1139:32 

418.6 

NaN 

1416.3 

1204.5 

727.2 

735.0 

1141:26 

435.3 

NaN 

1433.7 

NaN 

678.4 

NaN 

1144:05 

453.1 

NaN 

1427.7 

1133.4 

643.8 

NaN 

1146:12 

459.7 

NaN 

1439.5 

NaN 

595.3 

NaN 

1150:39 

499.8 

NaN 

1455.0 

1024.0 

528.7 

949.5 

1152:44 

541.2 

NaN 

NaN 

991.5 

488.2 

NaN 

1155:04 

567.4 

NaN 

1484.1 

971.1 

455.8 

1019.1 

1157:05 

NaN 

NaN 

1497.0 

NaN 

422.8 

1065.7 

1159:08 

619.6 

1581.1 

1510.0 

927.6 

404.8 

NaN 

1201:24 

667.9 

NaN 

1529.8 

903.7 

388.3 

1137.1 

1204:06 

716.1 

NaN 

1551.9 

865.3 

372.4 

NaN 

1211:32 

836.5 

NaN 

1617.9 

829.2 

361.8 

NaN 

1213:36 

986.4 

NaN 

1663.9 

808.8 

386.2 

NaN 

1220:46 

NaN 

NaN 

NaN 

433.3 

1480.9 

NaN 

Table  14.  Range  data  for  Slocum  track 


B,  SUBMARINE  TRACK  RECONSTRUCTION 

In  October  2004,  a  submarine  conducted  tests  with  a  Seaweb  network.  The  range 
data  from  this  exercise  are  analyzed  to  create  position  fixes  that  are  then  compared  to  the 
SSN’s  Ring  Laser  Gyro  Navigation  (RLGN)  position  estimates.  Calculated  speeds  from 
the  RLGN  data  vary  from  4.5  to  6.8kts,  significantly  faster  than  Seaweb  tests  with  UUVs. 
A  chart  of  this  track  is  shown  in  Figure  23. 
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Figure  23.  Submarine  track  reconstruction  chart 


As  with  the  UUV  experiments,  another  true  fix  source  was  not  available  for 
comparison  during  these  tests.  Flowever  15  of  the  16  Seaweb  fixes  fell  within  the 
standard  position  uncertainty  radius  of  Inm  from  the  RLGN  reading  taken  at  the  same 
time.  The  largest  position  difference  was  approximately  3350m,  occurring  at  the  course 
change  in  the  track.  The  RLGN  position  for  this  time  is  further  to  the  north  than  the 
Seaweb  fix.  This  fix  occurs  on  the  edge  of  the  network  in  an  area  with  poor  GDOP 
characteristics  and  as  such  is  less  likely  to  be  accurate.  The  good  performance  while 
operating  in  the  interior  of  the  network  does  show  the  utility  of  using  an  acoustic  network 
as  an  underwater  positioning  system. 

During  this  event,  16  ping  requests  were  sent  and  55  total  responses  were  received 
from  17  nodes.  From  these  responses,  38  valid  position  fixes  are  generated. 
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VIII.  CONCLUSIONS  AND  RECOMMENDATIONS 


A.  CONCLUSIONS 

Despite  the  sometimes  sparse  data  diseussed  in  Chapter  VII,  the  differenee 
linearization  algorithm  and  the  available  ranges  eonsistently  result  in  high  quality  fixes 
for  an  underwater  positioning  system.  The  linear  relationship  observed  in  Chapter  V 
between  the  distributions  of  range  error  and  position  error  is  a  good  step  in  eharaeterizing 
the  quality  of  the  position  fixes. 

B,  RECOMMENDATIONS  FOR  FUTURE  WORK 

1,  Test  with  Vehicle  in  Receipt  of  GPS 

Although  the  simulations  in  Chapters  V  and  VI  indieate  that  the  differenee 
linearization  algorithm  performs  better  than  previous  fix  methods,  at-sea  verifieation  has 
been  limited  due  to  the  faet  that  none  of  the  vehieles  tested  were  reeording  simultaneous 
fixes  from  a  trusted  source  such  as  GPS.  Comparisons  to  the  vehicles’  onboard  inertial  or 
dead-reckoning  navigation  systems  have  indicated  that  Seaweb  ranges  can  be  used  to 
obtain  a  reasonable  position  fix.  Experimental  control  with  a  vehicle  simultaneously 
recording  both  GPS  and  Seaweb  fixes  would  allow  for  determination  of  the  absolute 
quality  of  the  Seaweb  fixes. 

2.  Algorithms  that  can  Incorporate  Asynchronous  Ranges 

Experiments  to  date  have  used  a  broadcast  ping  from  the  mobile  node  to  all  fixed 
nodes  in  range  to  obtain  the  position  fix.  As  discussed  in  Chapter  I,  a  range  is  calculated 
for  each  data  transmission.  These  data  transmissions  are  normally  sent  in  accordance 
with  a  routing  table  resulting  in  ranges  from  only  a  few  fixed  nodes  at  a  time  as  opposed 
to  all  nodes  for  a  broadcast  ping.  In  actual  network  operations,  these  data 
communications  may  occur  more  frequently  than  broadcast  requests.  An  algorithm  that 
could  incorporate  asynchronous  range  data  from  such  communications  would  increase 
the  utility  of  the  positioning  system.  This  would  also  address  the  ability  to  calculate 
position  fixes  in  the  cases  of  poor  broadcast  ping  response  when  only  one  or  two  fixed 
node  responses  are  received. 
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3,  GDOP  Weighting 

As  stated  in  Chapter  III,  the  GDOP  performanee  of  a  given  set  of  nodes  ean  be 
quantitatively  assessed  through  the  eondition  number  of  the  P  matrix  of  equation  (3.3).  A 
possible  method  for  improving  the  overall  algorithm  would  involve  a  weighting  seheme 
based  on  the  eondition  number  of  the  assoeiated  node  set.  With  this  method,  solutions 
that  are  expeeted  to  be  good  due  to  better  GDOP  eharaeteristies  would  be  more  heavily 
favored  than  those  from  node  sets  with  weaker  GDOP  performanee. 

4,  Programming  Inefficiencies 

The  eombination  algorithm  is  aetually  the  slowest  of  the  four  methods  presented, 
though  still  fast  enough  for  real-time  use.  Run  time  is  a  funetion  of  several  variables,  but 
one  of  the  most  signifieant  is  the  number  of  fixed  nodes  in  the  network,  whieh  determines 
the  number  of  eombinations  for  whieh  solutions  will  be  ealeulated.  In  a  very  large 
network  it  is  likely  that  a  mobile  node  will  only  be  in  range  of  a  small  fraetion  of  fixed 
nodes  at  any  given  time.  While  enough  data  should  still  be  available  for  obtaining 
aeeurate  position  fixes,  it  would  not  be  neeessary  to  attempt  to  ealeulate  positions  from 
the  more  distant  nodes.  An  algorithm  that  uses  the  eurrent  position  of  the  mobile  node 
within  the  network  to  determine  whieh  fixed  nodes  to  interrogate  for  ranges  would 
improve  the  speed  performanee  of  the  algorithm  in  large  networks. 

5,  Kalman  Filter 

Integrating  this  algorithm  with  a  Kalman  filter  would  allow  for  predietion  of  the 
mobile  node’s  position  and  eould  also  improve  positioning  aeouraey.  The  motion  model 
in  the  Kalman  filter  eould  be  used  to  eliminate  position  fixes  that  do  not  make  sense  in 
the  eontext  of  the  mobile  nodes  traek  history  or  other  physieal  eonstraints. 

6,  Integration  with  Other  Navigation  Sensors 

The  aeeuraey  of  the  positioning  system  eould  also  be  improved  by  ineluding  data 
from  other  navigation  sensors  on  board  the  platform,  sueh  as  eourse,  speed,  and  depth. 
Similar  to  the  Kalman  filter,  these  data  would  improve  the  algorithm’s  ability  to  prediet  a 
future  position  and  eompare  this  predietion  with  ealeulated  fixes. 

7,  Modeling  and  Testing  for  Error  Estimation 

The  synthetie  data  presented  in  Chapters  V  and  VI  give  a  predietion  of  position 
error  when  the  statisties  of  the  range  errors  are  known.  In  praetiee,  these  range  errors  are 
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difficult  to  predict.  A  model  that  eould  prediet  range  errors  as  a  funetion  of  sound  speed, 
water  depth,  and  other  faetors  would  inerease  the  usefulness  of  the  insights  gained  in  the 
synthetie  testing. 
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APPENDICES 


A,  MATLAB  PROGRAMS  DEVELOPED  FOR  THIS  THESIS 

function  [a  b]  =  uuv  main 

q, 

o 

%  Thesis  Main  Program  for  difference  linearization  algorithm,  can  be 
run  as 

%  a  function  for  comparison  to  other  algorithms.  Output  matrix  a  is 
%  position  fix  matrix  for  combination  method,  b  is  position  fix  matrix 
for 

%  overdetermined  method. 

q, 

o 

%  Mike  Reed 

%  Naval  Postgraduate  School  Master's  Thesis 
%  June  2006 

q, 

o 


global  num  dims  all  nodes  num  nodes  range  data; 

%  clear 
%  close  all; 


%  get  user  input  for  node  file,  range  file,  and  number  of  dimensions  to 
%  trilaterate  (2  or  3) 

%  node_file  =  input ( [ ' \nPlease  enter  the  name  of  the  file  that  contains 


'the  fixed  node  positions.  \n'  'The  current  working  directory  is 


%  pwd  '/.\n\n'],  's'); 

%  node_mode  =  input ( [ ' \nPlease  enter  the  format  for  the  node 
position ' . . . 

%  'file.  [l]\n'... 

%  '1  -  Degrees  Minutes  Seconds  \n'  ... 

%  '2  -  Decimal  Degrees  \n\n']); 

%  %  default  conditional  goes  here  and  loop  to  ensure  1  or  2  is  entered 
%  num_dims  =  input ( [ ' \nPlease  enter  the  number  of  dimensions  for  ' . . . 

%  'position  fixing,  2  or  3.  \n\n']); 

%  %  default  conditional  goes  here  and  loop  to  ensure  2  or  3  is  entered 
%  range  file  =  input ( [ ' \nPlease  enter  the  name  of  the  file  that 
contains  '  .  .  . 

%  'the  ranging  data.  \n'  'The  current  working  directory  is  '... 

%  pwd  '/.\n\n'],  's'); 

q, 

o 

q, 

o 

q, 

o 


num  dims  =  2 ; 


q,  q, 
o  o 


q,  q, 

o  o 

q,  q, 

o  o 

q,  q, 

o  o 


over_determined  =  0; 

input  node  positions 

I nodel_x, 
I node2  x. 


so 

nodel_y, 

node2_y. 


nodel_z | 
node2  z | 
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o\°  o\°  o\o  o\°  o\o  o\o  o\o  o\o  o\°  o\o  o\o  o\°  o\o  o\°  o\°  o\o  ^  o\o  o\°  o\°  o\o  o\°  o\o  o\°  o\°  o\°  o\°  o\°  o\°  o\°  P  o\o  o\°  o\°  o\o  o\°  o\°  o\°  o\°  o\°  o\°  o\° 


%  %  all_nodes  =  |node3_x,  node3_y,  node3_z | 
%  %  |node4_x,  node4_y,  node4_z | 
%  %  |node5_x,  node5_y,  node5_z | 
%  %  |node6_x,  node6_y,  node6_z | 

9-  0, 
o  o 


pos_file  =  input ('Enter  the  output  file  name.  \n',  's'); 

%  node_file  =  ' . . /Dec92005/node_gps . txt ' ; 
node_mode  =  2 ; 

node_file  =  ' . . /Dec92005/sean_nodes . txt ' ; 

%  node_file  =  ' . . /Dec92005/taswex_node . txt ' ; 
all_nodes  =  node_read (node_f ile,  node_mode) ; 
if  num_dims  ==  3 

heights  =[0-802-3  -33]'; 
all_nodes  =  [all_nodes  heights]; 

end 


um  nodes 


length (all_nodes (:,!)); 


need  to  get  data  in  form 

time_data  =  [timestamp ( 1 )  ,  timestamp (2 ) ,  ...  timestamp (N) ] 


range_data 


rangel  ( 1 ) , 
range2  ( 1 ) , 
range3  ( 1 ) , 
range!  ( 1 ) , 
ranges  ( 1 ) , 
ranges  ( 1 ) , 


rangel (2 ) , 
range2 (2 ) , 
range3 (2 ) , 
range! (2 ) , 
ranges (2 ) , 
ranges (2 ) , 


rangel (N) | 
range2 (N) [ 
ranges (N) 1 
range! (N)  1 
ranges (N) [ 
ranges (N) [ 


range_data  =  node_ranges; 

range_file  =  ' . . /simulations/rng_grid_200 . txt ' ; 
waypoints,  transposed  to  match  output  where  each  colums  is  an 
Y,  (Z)  ] 
set 

sym_track_b  =  [-1000  -1000  20;  -7S0  0  80;  7S0  0  80;  1000  1000  20] ' 
sym_track_c  =  [-1000  -1000  SO;  -7S0  0  SO;  7S0  0  SO;  1000  1000  SO] ' 
range_file  =  ' . . /Dec9200S/Node!out_b . txt ' ; 
all_data  =  dlmread (range_f ile,  '  '); 

range_data  =  dlmread (range_file,  '  ',  0,  3) '; 


a  =  l.OS; 

err  mult  =1;  %  +  0 . 02S*randn (size (range_data) ) ; 

err_add  =  10* (randn (size (range_data) ) ) ; 

max (max (err_mult) ) 

min (min (err_mult) ) 

mean (std (err_mult) ) 

range_data  =  err_mult  . *  range_data  +  err_add; 

time_data  =  all_data ( : , 1 : S) ; 
range_data  =  all_data ( : , 3 : end) ' ; 
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%  range  file  =  ' . . /Dec92005/taswex_data . xls ' ; 

%  range  data  =  xlsread (range_file) ; 

%  range_data  =  (range_data (2 : end,  :))  '  ; 

%  Jul  21  tracks 
%  all_data  =  xlsread  . . . 

%  ('../Sean/Thesis  Def inite/Read-in  excel 

files/program_jul21  ping  ranges'); 

%  range_data  =  all_data ( 86 : 106,  1:6)  '  ; 

time_data  =  [ 1 : length ( range_data ( 1 ,:))] ; 

%  now  calculate  positions  from  all  combinations  of  data  at  each  time 
step, 

%  then  calculate  a  center  of  mass  position  by  averaging  over  the 
%  combinations 

q, 

o 


%  delta_time  =  time_diff (time_data) ; 

set  mat  =  comb  gen (length (all  nodes(:,l)),  num  dims  +  1 ) ; 
num  combs  =  combination (num  nodes,  num  dims  +  1); 


mult_pos  =  [  ] ; 

mult  error  =  zeros (num  combs,  length (time  data)); 
for  time  count  =  1 : length (time  data) 


if  over  determined 

nan  ranges  =  sum (isnan (range  data ( :  ,  time  count))); 
if  (num  nodes  -  nan  ranges)  <  (num  dims  +  1) 
best_pos_od ( : , time_count)  =  NaN; 

else 


0)  ; 


[best_pos_od ( : ,  time_count) ,  err_vect]  = 
veh_pos_od (range_data ( : ,  time_count) 


end 

%  else 

for  set  count  =  l:num  combs 

set_list  =  set_mat (set_count,  :); 
range_set  =  range_data (set_list,  time_ 
node_set  =  all_nodes (set_list,  :); 
[mult_pos (set_count,  :,  time_count) ,  . 

mult_error (set_count,  time_count) ] 
veh_pos (range_set,  node_set,  0); 
end  %  for  set_count 

best_pos ( : ,  time_count)  = 

(nanmean (mult_pos ( 1 : end, : , time_count) ) ) ' ; 

%  end  %  if  over_determined 

end  %  for  time  count 


count) 


all  nodes. 


a  =  best_pos ' ; 
b  =  best_pos_od ' ; 

%  figure; 

%  hold  on; 
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%  if  nuin_diins  ==  2 

%  plot (all_nodes ( : , 1 ) ,  all_nodes ( : , 2 ) ,  ' rx ' ) ; 

%  plot(a(:,l),  a(:,2),  'b+-'); 

%  %  plot(b(:,l),  b(:,2),  'mo-.'); 

%  %  plot (mult_pos ( : , 1 ) ,  mult_pos ( : , 2 ) ,  'go'); 

%  %  plot ( sym_track_c ( 1 ,  : ) ,  sym_track_c (2 ,  : ) ,  ' kH - '); 

%  %  plot ( real_node6 ( 1 ) ,  real_node6 (2 ) ,  'go'); 

%  axis  equal; 

%  %  xlim( [-1200  1200] ) ; 

%  %  ylim( [-1200  1200] ) ; 

%  %  legend (' Fixed  Nodes',  'Mean  Combination  Fix',  'Overdetermined', 


%  %  'Individual  Fix',  'location',  'Best'); 

%  %  title('2-D  estimation'); 

%  else 

%  plots (all_nodes (:, 1 ) ,  all_nodes ( : , 2 ) ,  all_nodes ( : , 3 ) ,  ' rx ' )  ; 

%  plots (a ( : , 1) ,  a(:,2),  a(:,3),  'b.-'); 

%  %  plots (b (:, 1) ,  b(:,2),  b(:,3),  'mo-.'); 

%  %  plots ( sym_track_c ( 1 ,:) ,  sym_track_c (2 , : ) ,  sym_track_c ( 3 , : ) , 

'k+--' ) ; 

%  legend (' Fixed  Nodes',  'By  Combinations',  'Overdetermined',  ... 

%  'Waypoints',  'location',  'Best'); 

%  title ('3-D  estimation'); 

%  %  title (' Ranged  positions  for  node  7  without  gateway  ranges'); 

%  %  xlabel (' meters ') ; 

%  %  ylabel (' meters ') ; 

%  end 


function  b  =  node  read (a,  format) 

g, 

o 

%  gets  fixed  lat/long  position  from  file  a  in  either  degrees,  minutes, 
%  seconds  (format  =  1)  or  decimal  degrees  and  converts  to  local 
coordinate 

%  system  in  meters  (format  =2) .  Origin  of  the  local  coordinate  system 
is 

%  the  first  node  in  the  file. 

g, 

o 

%  Mike  Reed 

%  Naval  Postgraduate  School  Master's  Thesis 
%  June  2006 

g, 

o 

node_gps  =  dlmread(a); 

switch  format 
case  1 

%  use  following  two  lines  if  data  file  is  in  degrees,  minutes,  seconds 
%  node_lat  =  dms2deg (node_gps ( : , 1 ) ,  node_gps ( : , 2 ) ,  node_gps ( : , 3 ) ) ; 

%  node_long  =  dms2deg (node_gps ( : , 4 ) ,  node_gps ( : , 5 ) ,  node_gps ( : , 6) ) ; 
node_long  =  dms2deg (node_gps ( : , 1 ) ,  node_gps ( : , 2 ) ,  node_gps ( : , 3 ) ) 
node_lat  =  dms2deg (node_gps ( : , 4 ) ,  node_gps ( : , 5 ) ,  node_gps ( : , 6 ) ) 
case  2 

%  use  the  fillowing  two  lines  if  data  file  is  in  decimal  degrees 
node_lat  =  node_gps ( : , 1 ) ; 
node_long  =  node_gps ( : , 2 ) ; 
end 
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[latlen,  longlen]  =  lat  len (mean (node  lat) )  ; 


node_lat_rel_d  =  node_lat  -  node_lat ( 1 ) ; 
node  long  rel  d  =  node  long  -  node  long(l); 

node  lat  rel  m  =  node  lat  rel  d  *  latlen; 
node  long  rel  m  =  node  long  rel  d  *  longlen; 

b  =  [node  long  rel  m  node  lat  rel  m] ; 


function  [latlen,  longlen]  =  lat  len (lat) 

g, 

o 

%  taken  from  http://pollux.nss.nima.mil/calc/degree.html  conversion  to 
%  units  other  than  meters  also  available  there. 

g, 

o 

%  Compute  lengths  (in  meters)  of  degrees  of  latitude  and  longitude 
based 

%  on  latitude 

g, 

o 

%  Mike  Reed 

%  Naval  Postgraduate  School  Master's  Thesis 
%  June  2006 

g, 

o 


Convert  latitude  to  radians 
lat  =  deg2rad (lat) ; 


Set  up  "Constants" 
ml  =  111132.92; 

g, 

o 

latitude 

calculation 

term  1 

m2  =  -559.82; 

g, 

o 

latitude 

calculation 

term  2 

m3  =  1.175; 

g, 

o 

latitude 

calculation 

term  3 

m4  =  -0.0023; 

g, 

o 

latitude 

calculation 

term  4 

pi  =  111412.84; 

g, 

o 

longitude 

calculation 

term  1 

p2  =  -93.5; 

g, 

o 

longitude 

calculation 

term  2 

p3  =  0.118; 

g, 

o 

longitude 

calculation 

term  3 

%  Calculate  the  length  of  a  degree  of  latitude  and  longitude  in 

meters 

latlen  =  ml  +  (m2  *  cos (2  *  lat)  )  +  (m3  *  cos  (4  *  lat) )  +  . . . 
(m4  *  cos ( 6  *  lat) ) ; 

longlen  =  (pi  *  cos (lat))  +  (p2  *  cos (3  *  lat))  +  ... 

(p3  *  cos  (5  *  lat) ) ; 


function  b  =  comb  gen(n,  k) 

g, 

o 

%  b  =  comb  gen(n,  k) 

g, 

o 

%  Generates  all  combinations  (order  does  not  matter)  of  l:n  taken  k 
%  elements  at  a  time.  Each  element  is  listed  as  a  row  of  the  output 
%  matrix. 
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%  This  algorithm  is  taken  from  Discrete  Mathematics  and  Its 
Applications , 

%  by  Kenneth  H.  Rosen,  Random  House,  New  York,  NY  1988  (first  edition) 
%  p.  227 

q, 

o 

%  uses: 

%  combination ( ) 

q, 

o 

%  used  by: 

%  uuv_main ( ) 

q, 

o 

%  Mike  Reed 

%  Naval  Postgraduate  School  Master's  Thesis 
%  June  2006 

q, 

o 


num  combs  =  combination (n, k) ; 

comb  vect  =  l:k;  %  first  line 

comb  mat  =  comb  vect; 

for  comb  count  =  2 : num  combs 
elem  count  =  k; 

while  comb  vect (elem  count)  ==  (n  -  k  +  elem  count) 
elem  count  =  elem  count  -  1; 

end 

comb  vect (elem  count)  =  comb  vect (elem  count)  +  1; 
count  2  =  (elem  count  +  1) :k;  %  Matlab  will  not  create 

decrementing 

%  vectors  unless  the  decrement  is  specified,  ie  5:1  will 

NOT 

%  create  [5  4  3  2  1]  but  4:-l:l  would  create  [4  3  2  1]  . 

Thus, 

%  if  elem_count  >=  k,  count_2  will  be  an  empty  matrix  and 

the 

%  following  line  indexing  on  count_2  will  be  ignored, 
comb  vect (count  2)  =  comb  vect (elem  count)  +  count  2  -  elem  count; 
comb  mat  =  [comb  mat;  comb  vect] ; 

end 

b  =  comb  mat; 


function  [b,  error  out]  =  veh  pos (ranges,  node  pos,  error  in) 


%  [b,  error]  =  veh_pos (range,  node_pos) 

q, 

o 

%  triangulate  the  cartesian  coordinates 
%  triangulating  given  the  straight  line 
unknown 

%  point  from  n+1  knownn  locations  (node 

q, 

o 

%  The  set  of  equations  to  be  solved  for 

q, 

o 

%  rl'^2  =  (x-xl)^2  +  (y-yl)'^2 


of  a  point  b  in  n  dimensions  by 
distances  (ranges)  to  the 

pos)  . 

(x,y)  are  of  the  form 


where  rl  is  an  element  of  ranges  and  (xl,yl)  are  given  by  a  row  of 
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%  node  pos) .  These  equations  are  solved  by  subtracting  pairs  of 
equations 

%  which  eliminates  squared  terms  of  the  unknowns  and  leads  to  equations 
of 

%  the  form 

q, 

o 

%  [x]  =  (2*(x2  -  xl))\((rl'^2  -  r2^2)  -  (xl'^2  -  x2^2)) 

q, 

o 

%  (see  thesis  report  for  full  derivation) 

q, 

o 

%  since  this  method  relies  on  the  difference  between  positions,  it  can 
%  return  a  position  even  if  the  range  circles  don't  intersect,  so  we 
check 

%  the  calculated  position  with  range  check (),  which  solves  for  ranges 
from 

%  the  calculated  position  to  the  known  locations.  range_check  places  a 

'1 ' 

%  in  the  error  vector  if  the  position  does  not  match  with  the  measured 
%  ranges.  If  any  input  range  is  zero  or  NaN,  the  output  position  is 
%  returned  as  a  vector  of  NaN  values  and  a  '2'  is  placed  in  the  error 
%  vector. 

q, 

o 

%  uses: 

%  sqr_diff(),  pos_diff(),  range_check ( ) ,  nan_tester() 

q, 

o 

%  Mike  Reed 

%  Naval  Postgraduate  School  Master's  Thesis 
%  June  2006 

q, 
o 

error  out  =  error  in; 

range  diff  sqr  =  sqr  diff (ranges) ; 
node_diff  =  pos_dif f (node_pos ) ; 
node  diff  sqr  =  sqr  diff (node  pos); 
node  diff  inv  =  pinv(2*node  diff); 

diff  sqr  vect  =  range  diff  sqr  -  node  diff  sqr; 

b  =  2*node  diff  \  (range  diff  sqr  -  node  diff  sqr) ; 

[b,  error_out]  =  range_check (b,  ranges,  node_pos); 
if  error  out  ~=  0 

b  =~[NaN  NaN] ; 
end  %  if  range  error 


%  (rl"2  -r2"2) 

%  (x2  -  xl) 

%  (xl^2  -  x2'^2) 


function  [b,  error  out]  =  veh  pos  od (ranges,  node  pos,  error  in) 

q, 

o 

%  [b,  error]  =  veh_pos (range,  node_pos) 

q, 

o 

%  triangulate  the  cartesian  coordinates  of  a  point  b  in  n  dimensions  by 
%  triangulating  given  the  straight  line  distances  (ranges)  to  the 
unknown 

%  point  from  n+1  knownn  locations  (node  pos) . 

q, 

o 

%  The  set  of  equations  to  be  solved  for  (x,y)  are  of  the  form 
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%  rl''2  =  (x-xl)^2  +  (y-yl)'^2 

g, 

o 

%  where  rl  is  an  element  of  ranges  and  (xl,yl)  are  given  by  a  row  of 
%  node  pos) .  These  equations  are  solved  by  subtracting  pairs  of 
equations 

%  which  eliminates  squared  terms  of  the  unknowns  and  leads  to  equations 
of 

%  the  form 

g, 

o 

%  [x]  =  (2*(x2  -  xl))\((rl^2  -  r2''2)  -  (xl^2  -  x2''2)) 

g, 

o 

%  (see  thesis  report  for  full  derivation) 

g, 

o 

%  since  this  method  relies  on  the  difference  between  positions,  it  can 
%  return  a  position  even  if  the  range  circles  don't  intersect,  so  we 
check 

%  the  calculated  position  with  range  check (),  which  solves  for  ranges 
from 

%  the  calculated  position  to  the  known  locations.  range_check  places  a 

'  1 ' 

%  in  the  error  vector  if  the  position  does  not  match  with  the  measured 
%  ranges.  If  any  input  range  is  zero  or  NaN,  the  output  position  is 
%  returned  as  a  vector  of  NaN  values  and  a  '2'  is  placed  in  the  error 
%  vector. 

g, 

o 

%  uses: 

%  sqr_diff(),  pos_diff(),  range_check  ( )  ,  nan___tester  ( ) 

g, 

o 

%  Mike  Reed 

%  Naval  Postgraduate  School  Master's  Thesis 
%  June  2006 

g, 

o 


error  out  =  error  in; 

%  find  and  remove  NaN's  before  creating  pos_diff  and  sqr_diff 
nan  index  =  f ind ( isnan ( ranges )) ; 
ranges (nan_index)  =  [ ] ; 
node_pos (nan_index,  : )  =  [  ]  ; 
if  length ( ranges )  >  2 

g, 

o 

range  diff  sqr  =  sqr  diff  (ranges)  ;  %  (rl''2  -r2''2) 

node  diff  =  pos  diff (node  pos);  %  (x2  -  xl) 

node  diff  sqr  =  sqr  diff  (node  pos);  %  (xl''2  -  x2^2) 

node  diff  inv  =  pinv(2*node  diff); 

diff  sqr  vect  =  range  diff  sqr  -  node  diff  sqr; 


b  =  2*node  diff  \  (range  diff  sqr  -  node  diff  sqr); 

[b,  range_error]  =  range_check (b,  ranges,  node_pos); 
if  range  error  ~=  0 

error_out  =  [error_out,  range_error] ; 
end  %  if  range_error 
%  end  %  if  nan  tester 
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else 


b  =  NaN; 

range  error  =2; 

end 


function  b  =  pos_diff (a) 

g, 

o 

g, 

o 

%  returns  a  matrix  that  is  the  difference  of  the  rows  of  the  input 
matrix . 

%  In  general,  if  a  has  dimensions  m  x  n,  b  will  have  dimensions  (m-1)  x 

n . 

g, 

o 

%  |xl  yl I  t  (x2  -  xl),  (y2  -  yl)| 

%  If  a= I x2  y2  I  then  pos_diff  returns  b  =  |  (x3  -  x2),  (y3  -  y2 )  | 

%  Ix3  y3| 

g, 

o 

%  nb  that  pos  diff  subtracts  the  upper  row  from  the  lower  row,  which  is 
%  opposite  from  the  operation  of  sqr_diff.  pos_diff  requires  that  the 
%  input  matrix  a  have  at  least  2  row  but  supports  any  higher  number  of 
%  dimensions. 

o 

o 

%  used  by: 

%  veh_pos ( ) 

g, 

o 

%  Mike  Reed 

%  Naval  Postgraduate  School  Master's  Thesis 
%  June  2006 

g, 

o 


b  =  a (2  tend,  :)  -  a(l:end-l,  : ) ; 


function  b  =  sqr  diff (a) 

g, 

o 

g, 

o 

%  returns  a  column  vector  of  the  differences  of  squared  range  or 
position 
%  data. 

%  I  al  I  I  al''2  -  a2^2  | 

%  If  a=  I  a2  I  then  sqr_diff  returns  b=|a2''2  -  a3'^2  | 

%  I  a3  I 

g, 

o 

%  |xl  yl|  I  (xl^2  -  x2'^2)  +  (yl^2  -  y2'^2)| 

%  If  a=  I  x2  y2  I  then  sqr_diff  returns  b=  |  (x2''2  -  x3''2)  +  (y2''2  -  y3^2)  | 

%  Ix3  y3| 

g, 

o 

%  nb  that  sqr  diff  subtracts  the  lower  row  from  the  upper  row,  which  is 
%  opposite  from  the  operation  of  pos  diff.  sqr_diff  requires  that  the 
%  input  matrix  a  have  at  least  2  rows  but  supports  any  higher  number  of 
%  dimensions. 

g, 

o 

%  used  by: 

%  veh_pos()  in  both  forms  above. 
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%  Mike  Reed 

%  Naval  Postgraduate  School  Master's  Thesis 
%  June  2006 

g, 

o 


sqr_mat  =  a(l:end-l,  •.)  .''2  -  a(2:end,  :)  .''2; 
b  =  sum (sqr_mat,  2); 


function  [pos_out,  error]  =  range_check (pos_in,  range,  node_pos) 

g, 

o 


%  b  =  range_check (position,  range,  node  pos) 

g, 

o 

%  checks  that  the  straight  line  differences  between  a  calculated  point 
%  (position)  and  a  set  of  given  fixed  locations  (node_pos)  is  equal  to 
%  measured  ranges  (range)  within  an  acceptable  error 

g, 

o 

%  returns  0  if  the  calculated  range  and  measured  range  match 
%  returns  1  if  the  calculated  range  and  measured  range  do  not  match 

g, 

o 

%  uses: 

%  veh_pyth ( ) 

g, 

o 

%  used  by: 

%  veh_pos ( ) 

g, 

o 

%  Mike  Reed 

%  Naval  Postgraduate  School  Master's  Thesis 
%  June  2006 

g, 

o 


acc_error  =0.1;  %  acceptable  error  as  a  percentage  of  range 

if  abs( (range  -  veh_pyth (pos_in,  node_pos) ). /range)  ... 

<  acc  error 


error  = 

0;  %  position  and 

range 

values 

check 

error  = 

1 ;  %  position  and 

range 

values 

do  not  check 

for  i  = 

1: length (pos  in(:,l)) 

for 

j  =  l:length(pos  in(l,; 

:)  ) 

if  abs (pos  in(i,j))  == 

inf 

pos_in(i,j)  =  NaN; 
end  %  if  a 


end  %  for  j 
end  %  for  i 
end  %  if 

pos_out  =  pos_in; 

function  b  =  veh  pyth (position,  node  pos) 

g, 

o 

%  b  =  veh_pyth (position,  node_pos) 

g, 

o 

%  uses  the  Pythagorean  theorem  to  calculate  the  ranges  from  a  point 
%  (position)  to  each  of  a  set  of  fixed  nodes  with  locations  given  by 
the 

%  rows  of  node_pos. 
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0, 

o 

Ix| 

\ — 1 
>1 

( — 1 
X 

g, 

o 

if  position  = 

1 y 1  and 

node  pos 

= 

1 x2  y2 1  then  veh  pyth 

returns 

g, 

o 

g. 

|x3  y3 1 

o 

g, 

o 

1  (X  - 

xl)  ^2 

+ 

(y 

-  yi)' 

'2  1 

g, 

o 

b  =  sqrt 1 (x  - 

x2)  ^2 

+ 

(y 

-  y2)' 

'2  1 

such  that  each  row  i 

of  b  is  the 

g, 

o 

g. 

1  (X  - 

x3)  ^^2 

+ 

(y 

-  y3) ' 

'2  1 

o 

g, 

o 

straight  line 

range 

from 

(x,  y) 

to 

the  location  (xi,yi) 

given  in  row 

of 

%  node_pos.  veh_pyth  supports  any  number  of  dimensions  and  fixed 
points . 

g, 

o 

%  used  by: 

%  range  check () 


%  Mike  Reed 

%  Naval  Postgraduate  School  Master's  Thesis 
%  June  2006 


dims  =  length (node  pos(l, :));  %  dims  =  number  of  rows  in  node  pos 

num_nodes  =  length (node_pos (:, 1 )) ; 

for  count  =  l:length(position(l,:)); 

distance count)  =  ones (num_nodes , 
dims) *diag (position ( : , count) ) ; 
end 

for  count  =  l:length(position(l,:)); 

distance  (:,:,  count)  =  (distance  (:,:,  count)  -  node_pos )  .  "^2 ; 

end 

b  =  sqrt ( sum (distance,  2)); 


B.  MATLAB  PROGRAMS  USED  BY  PREVIOUS  THESES  (WITH 
MODIFICATIONS  FOR  DATA  COMPARISON) 

1,  Center  of  Mass  Method  [4] 

function  fix  pos  =  complete  program 

%This  program  was  used  to  run  simulation  one  with  the  center  of  mass 
method 

%It  also  contains  all  parts  to  run  simulation  two  and  track  the  glider 
with 

%real  data 

global  all_nodes  range_data; 
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%  close  all  %closes  all  open  figure  windows 

%  clear  all  %clears  all  variables,  functions,  etc  from  memory 
%  clc  %clears  command  window 


%  path (path,  '../Read-in  excel  files'); 

%  %  inserted  data  read-in  code  from  uuv_main() 
num  dims  =2; 

%  %  node  file  =  'node  gps.txt'; 

%  node_mode  =  2 ; 

%  node_file  =  ' sean  nodes.txt'; 

%  %  node_file  =  'taswex_node.txt'; 

%  all_nodes  =  node__read  (node_f  ile,  node_mode)  ; 
node_pos  =  all_nodes; 
if  num  dims  ==  3 

heights  =[0-802-3  -33]'; 
node_pos  =  [node_pos  heights] ; 

end 

%  %  real_node4  =  all_nodes (2 ,  : ) 

%  %  real_node5  =  all_nodes ( 3 , : ) 

%  %  real_node6  =  all_nodes ( 4 , : ) 

%  %  real_node7  =  all_nodes ( 5 , : ) 

%  %  real_node8  =  all_nodes ( 6, : ) 

g,  g, 
o  o 

%  %  all_nodes  =  all_nodes ( [2  356],  :); 

%  num_nodes  =  length (all_nodes (:, 1 )) ; 

%  node  pos=xlsread ( ' program_stationary  nodes');  %node  pos  will  read 
from  an 

%excel  file  to  take  the  positions  of  the  nodes  that  were  used  in 

the 

%experiment 


%glider  pos=xlsread ( ' simulation2_inner ' ) ;  %used  for  the  simulation  of 
the 

%inner  track 

%glider  pos=xlsread ( ' simulation2_outer ' ) ;  %used  for  the  simulation  of 
the 

%outer  track 

%glider  pos=xlsread ( ' program_j ul2 0_glider  ins  pos');  %used  to  read  in 
the 

%dead  reckoning  positioning  data  from  the  Glider. 

%  glider  pos=xlsread ( ' program_j ul2 l_glider_ins  pos'); 

%  glider  pos  =  dlmread /simulations/pos  grid_truth.txt'); 
%glider  pos=xlsread ( ' program_j ul22_glider  ins  pos'); 

g, _ _ _ _ _ _ _ _ _  _ _ _ _ _ _ _  _ _ 

o 
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%gps=xlsread ( 'program_jul20_gps  fix');  %Iinports  excel  files  with  the 
gps 

%  gps=xlsread ( ' program_j ul21_gps_f ix ' ) ;  %data  for  each  day 
%gps=xlsread ( ' program_j  ul22_gps_f ix ' ) ; 

9~  —  —  —  —  —  —  —  —  —  —  —  —  —  —  —  —  —  —  —  —  —  —  —  —  —  —  —  —  —  —  — 

o 


%  This  loop  will  determine  the  node  to  node  ranges 
(internode_ranges (i, j ) ) , 

%  which  are  straight  line  distances,  as  well  as  those  ranges  translated 
to 

%  the  xy  plane.  This  length,  internode  ranges_xy  (or  rij  in  diagrams), 
will 

%  be  less  than  the  straight  line  distance  unless  the  two  nodes  are  at 
the 

%  same  depth. 


for (i=l : 1 : size (node_pos, 1) ) ; 

for ( j=l : 1 : size (node_pos,  1) ) ; 
if (i~=j ) ; 

x_dif f ( i ,  j ) =node_pos ( i , 1 ) -node_pos ( j , 1 ) ; 
y_dif f ( i , j ) =node_pos ( i , 2 ) -node_pos ( j , 2 ) ; 

%  z_dif f=node_pos (i, 3) -node_pos ( j , 3) ; 

internode_ranges  ( i ,  j  )  =sqrt  (x_dif  f  ( i ,  j  )  ''2+y_dif  f  ( i ,  j  )  '^2  )  ; 


9-  + 

g, 

o 

g, 

o 


z_dif  f  "'2  )  ; 

internode_ranges_xy  (i,  j  )  =sqrt  (  ( internode_ranges  (i,  j  )  ''2- 


z_diff'"2)  )  ; 

internode_ranges_xy ( i , j ) =internode_ranges ( i ,  j ) ; 


end 


end 


end 


%  G=input . . . 

%  ('Enter  the  number  of  times  the  glider  position  should  be 

simulated ' ) ; 

ping  ranges  =  range  data';  % 

dlmread ('../../../ simulations/ rng_grid_albO . txt ' ) ; 
ping  ranges  xy  =  ping  ranges; 

G=length (ping_ranges ( : , 1 ) ) ; 

%For  simulation  2,  G=1 : 1 : size (glider  pos,l). 

%  for  (i=l:l:G);  %make  a  list  of  positions  equal  to  G  for  the 
simulation 

%  glider_pos (i, 1) = ( . 5 -rand (1) ) *2000; 

%  glider_pos (i, 2) = ( . 5 -rand (1) ) *2000; 

%  glider_pos (i, 3) = (rand (1) *100) ; 

%  end 

%  To  find  the  ranges  that  would  be  present  between  the  glider  and  each 
node 
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%  a  matrix  is  created  to  determine  the  distances  with  some  random 
error,  as 

%  was  seen  in  the  experiment,  ping  ranges (g,  k)  looks  at  the  glider 
position 

%  for  each  (g)  and  gives  the  range  to  each  node  for  that  step  in  the 
loop 

%  (g=l:l:G) .  An  error  of  up  to  +/-  10m  is  used  and  added  to  the  ranges. 
%  ping_ranges  xy(l,k)  takes  the  range  when  transformed  down  to  the  xy 
plane 

%  as  described  in  Chapter  IV. 
for  (g=l : 1 : G) ; 

clear  xgood  ygood  yoga  xoga  yogb  xogb  xogab  yogab  %This 

%clears  all  variables  that  may  change  in  length  for  different 
%values  of  g. 


%ping_ranges=xlsread ( 'program_jul20  ping  ranges');  %These  commands 

read 

%in  files  which  contain  the  ping  ranges  for  each  day's  data 
%  ping  ranges=xlsread (' program  jul21  ping  ranges'); 

%ping_ranges=xlsread (' program  jul22_ping  ranges'); 

0, 

o 


%  for  (k=l ; 1 : size (node_pos, 1) ) ; 

%  ping_ranges  (g)  =sqrt  (  (node_pos  ( k,  1 )  -glider_pos  (g,l))''2+... 

%  (node_pos ( k, 2 ) -glider_pos (g,2) ) ^2+ . . . 

%  (node_pos  (k,  3)  -glider_pos  (g,  3)  )  ''2)  ; 

%  %ping  error (g)=0;  %No  ping  error  was  used  for  figures  27  and 

28 

%  ping_error (g) =20* (. 5-rand (1) ) ;  %Used  for  a  +/-  10m  range 

error 

%  ping_ranges (g) =ping_ranges (g) +ping_error (g) ; 

%  ping_ranges_xy (g, k) =sqrt ( (ping_ranges (g) ) ^2- (node_pos (k, 3) - 

%  glider_pos  (g,  3 )  )  "'2 )  ; 

%  end 

%  Now  the  ranges  between  each  node  as  well  as  the  ranges  from  each 

node 

%  to  the  glider  have  been  determined  at  every  step  from  1:1:G. 

These 

%  have  also  all  been  translated  onto  the  xy  plane.  The  next  step  is 
to 

%  find  which  triangles  form  real  solutions.  While  all  nodes  would 

have 

%  ranges  in  this  simulation,  not  all  ranges  were  found  at  every 

time 

%  step  in  the  experiment.  Solution  are  only  going  to  be  present  for 
%  range  circles  that  touch,  though.  The  situation  described  in 
Chapter 

%  IV,  section  E,  part  1  will  not  be  taken  into  account  since  it  has 
no 

%  real  affect  on  the  final  solution 
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for  (i=l : 1 : size (node_pos, 1) ) ; 
rl=ping_ranges_xy (g, i ) ; 
for  ( j=l : 1 : size (node_pos, 1) ) ; 
if (i~=j ) ; 

r2=internode  ranges  xy(i,j);  %These  give  the  three 
r3=ping_ranges_xy (g, j ) ;  %sides  of  the  triangle 

theta ( i , j ) =law_of_cosines ( rl , r2 , r3 ) ;  %theta  will 

give 

%the  angle  between  the  two  nodes  and  node  i  to  the 
%glider (Chapter  IV) 
if (isreal (theta (i, j ) ) ==1) ; 

theta_good ( i , j ) =theta ( i , j ) ; 

else 

theta_good ( i , j ) =NaN; 

end 

end 

end 

end 

%  Each  good  pair  of  nodes  (that  also  have  ping  ranges)  will  give  an 
xy 

%  solution.  Because  of  errors  and  delay  times,  these  positions  will 

not 

%  be  the  same  as  for  all  other  pairs'  solutions.  Factoring  all  of 

them 

%  together  will  lead  to  a  real,  approximate  solution.  Values  for 
%  theta_good  for  all  solutions  at  each  ping  time  have  now  been 
%  calculated.  The  following  will  calculate  phi  and  then  gamma  for 

each 

%  solution. 


count=0 ; 

for (i=l : 1 : size (node  pos,l));  %i  from  1  to  7 

for (j=i+l : 1 : size (node_pos, 1) ) ;  %Only  compare  the  1st  node  to 

all 

%others  and  then  the  second  to  nodes  3-7,  and  so  on 
if  isnan (theta_good (i, j ) ) ==0  &  (i~=j);  %if  theta_good  is 

not 

%NaN,  then  look  at  that  i,j  combination 
count=count+l ; 

if  (x_dif f ( i , j ) ==0 ) ;  %if  both  nodes  happen  to  lay 

directly 

%n-s  of  each  other  (not  necessary  for 

this 

%experiment,  but  may  be  needed  for  a 

future 


%experiment,  and. . . 

if  (node_pos ( i , 2 ) <node_pos ( j  ,  2 ) )  ;  %the  y 

%value  of  node  i  is  less  than  the  y  value  of 
%node  j  (figure  17) . . . 

phi  prime=pi/2;  %then  phi  (the  angle  between 

the  X 


%axis  and  the  line  between  the  two  nodes 


with 


node  i  as  the  origin)  is  pi/2)  and... 
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%If 

phi ( i , j ) =phi  prime ; 

else  phi  prime=-pi/2;  %If  the  y  value  of  node  i  is 
%larger,  then  phi  is  -pi/2 
phi ( i , j ) =phi  prime ; 

end 

else  phi  prime=atan (abs (y  dif f ( i , j ) ) /abs (x  dif f ( i , j ) ) ) ; 

%  the  X  difference  is  NOT  0  between  the  two  nodes, 

%  and. . . 

if  ((node  pos ( i , 1 ) <node  pos  ( j , 1 ) ) &  .  .  . 

(node  pos ( i , 2 ) <=node  pos(j,2))); 
phi(i,j)=phi  prime;  %In  first  quadrant 
el seif ( (node  pos ( i , 1 ) >node  pos ( j , 1 ) ) . . . 

& (node  pos ( i , 2 ) <=node  pos(j,2))); 
phi ( i , j ) =pi-phi  prime;  %In  second  quadrant 
elseif ( (node  pos ( i , 1 ) >node  pos ( j , 1 ) ) & . . . 

(node  pos ( i , 2 ) >=node  pos(j,2))); 
phi ( i , j ) =pi+phi  prime;  %In  third  quadrant 
elseif ( (node  pos ( i , 1 ) <node  pos ( j , 1 ) ) & . . . 

(node  pos ( i , 2 ) >=node  pos(j,2))); 
phi ( i , j ) =-phi  prime;  %In  fourth  quadrant 

end 

end 

nodes 

gamma  a ( i , j ) =phi ( i , j ) itheta  good(i,j);  %Each  set  of 

%will  give  two  values  of  gamma  corresponding  to  the 

two 

%solutions 

gamma  b ( i , j ) =phi ( i , j ) -theta  good(i,j); 

solutions 

%  The  following  part  will  be  used  for  finding  the 

from 

%  using  the  center  of  mass  method,  xoga  is  the  x  value 

%  the  center  node  to  the  glider  for  solution  a.  Since 
%  either  solution  a  or  b  will  be  good,  only  one  will  be 
%  kept 

the 

xoga (count) =node  pos ( i , 1 ) fping  ranges  xy(g,i)*... 

cos (gamma  a(i,j));  %This  is  the  first  x  value  of 

the 

%solution  for  every  i,j  combination 
xogb (count) =node  pos ( i , 1 ) +ping  ranges  xy(g,i)*... 

cos (gamma  b(i,j));  %This  is  the  second  x  value  of 

%solution  for  node  i 

end 

end 

end 

yoga (count) =node  pos ( i , 2 ) +ping  ranges  xy(g,i)*... 
sin (gamma  a(i,j)); 

yogb (count) =node  pos ( i , 2 ) +ping  ranges  xy(g,i)*... 
sin (gamma  b(i,j)); 

%  Calculate 

center  of  mass  for  all  the  points 

56 


xogab=[xoga  xogb] ;  %Combines  all  xoga  and  xogb  into  one  vector 
yogab=[yoga  yogb] ; 


cmx=naninean (xogab) ;  %nanmean  takes  the  mean  of  all  numbers  and 
cmy=nanmean (yogab) ;  %doesn't  account  for  anything  that  is  NaN 

%  Now,  throw  out  the  point  that  is  farther  away  from  the  center  of 

mass 

%  between  the  two  [(xoga, yoga)  or  (xogb, yogb)] 


count=l;  %A  counter  is  started  that  will  only  increase  by  1  every 
%time  a  certain  condition  is  met 
for  c  =  1: count; 

cmadiff(c)=  sqrt((cmx  -  xoga  (c))'' 2  +  (cmy  -  yoga  (c)  )  "'2 )  ; 

%Calculates  the  distance  between  the  center  of  mass  and 
%solution  a 

cmbdiff(c)=  sqrt((cmx  -  xogb (c)) ^2  +  (cmy  -  yogb (c) ) ^2 ) ; 
if  cmadif f (c) ==NaN  %For  an  xoga, yoga  pair  that  is  not  a 

number 

% (happens  when  the  range  errors  shrink  the  ranges  so  that 

theta 

%is  imaginary)  ,  make  the  distance  to  the  center  of  mass 

very 

%large 

cmadif f (c) =10000; 
break 

end 

if  cmbdif f (c) ==NaN; 

cmbdiff (c) =10000; 
break 

end 

if  cmadiff (c)  <=  cmbdiff (c) ;  %Only  use  the  point  that  is  closer 
of 


%the  two 
count=count+l ; 
xgood (count)  = 
ygood (count)  = 

else 

count=count+l ; 
xgood (count)  = 
ygood (count)  = 

end 


xoga (c) ; 
yoga (c) ; 


xogb (c) ; 
yogb (c) ; 


end 


cmx2=nanmean (xgood) ;  %Take  the  new  center  of  mass  of  the  good 
points 

cmy2=nanmean (ygood)  ; 

good_count=0 ; 
for  c  =  1: count; 

cmadiff (c)=  sqrt((cmx2  -  xoga (c)) ^2  +  (cmy2  -  yoga (c)) ^2); 
cmbdiff  (c)=  sqrt((cmx2  -  xogb  (c))"' 2  +  (cmy2  -  yogb  (c)  ) ''2 )  ; 
if  cmadif f (c) ==NaN 
cmadiff (c) =10000; 
continue 
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end 

if  cmbdif f (c) ==NaN; 

cmbdiff (c) =10000; 
continue 

end 

if  cmadiff (c)  <=  cmbdiff (c) ; 
good_count=good_count  +  1; 
xgood (good_count)  =  xoga(c); 
ygood (good_count)  =  yoga(c); 

else 

good_count=good_count  +  1; 
xgood (good_count)  =  xogb(c); 
ygood (good_count)  =  yogb(c); 

end 

end 

%  Now,  take  another  center  of  mass  to  calculate  the  final  position 
xfinal2 (g) =nanmean (xgood)  ; 
yfinal2 (g) =nanmean (ygood) ; 

sdxfinal=nanstd (xgood)  ; 
sdyfinal=nanstd (ygood) ; 


%The  following  part  was  used  to  run  simulation  one  with  the 
weighting 

%method 


%W=zeros (size (xogab,  1)  ,  1)  ; 

%for(i=l;l:size (xogab,  1) )  ; 

%for(j=i  +  l:l:size (xogab,  1) )  ; 

%f actor=l 00 ; 

%delta_x ( i , j ) =xogab ( i ) -xogab ( j ) ;  %Find  the  difference  in  x 
%position  between  every  solution  and  every  other 

solution 

%delta_y ( i , j ) =yogab ( i ) -yogab ( j ) ; 

%dxy=  (delta_x  ( i ,  j  )  )  ''2+  (delta_y  ( i ,  j  )  )  ''2; 

%if  dxy>=l;  %For  two  solutions  that  are  far  from 

%W ( i , j ) = ( 1 /dxy)  %each  other,  this  is  the  weight  value 
%else  %If  they  are  very  close,  they  get  a 

weight 

%W ( i , j ) =f actor ;  %of  100 

%end 

%end 


%end 

%Wsum=sum (nansum (W) ) ;  %Take  the  sum  of  all  of  the  weights.  Since  W 

%will  be  a  2  dimensional  matrix,  sum  up  the 
%columns  first,  and  then  sum  those  up 


%Each  xogab, yogab  solution  now  has  a  weight.  Taking  each  solutions 
%weight  and  dividing  by  the  total  weight  will  give  a  percentage  of 
%total  weight  for  that  solution.  Multiplying  that  percentage  by  its 

X 

%and  y  position  will  contribute  to  the  final  solution.  The  higher 

the 
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weight  percentage,  the  higher  the  contribution. 


%for  i=l : 1 : length (xogab) ; 

%for  j =1 : 1 : length (xogab) ; 

%x_fin (i, j ) =W (i, j ) /Wsum*xogab (i) ; 
%y_fin (i, j ) =W (i, j ) /Wsum*yogab (i)  ; 

%end 

%end 

%xf inal2=sum (nansum (x_f in) )  ; 

%yf inal2=sum (nansum (y_f in) ) ; 

g,_ _ _  _ _  __  _ 

o 


hold  on 


0, 

o 


to 


plot (glider_pos (g, 1 ) , glider_pos (g, 2 ) , ' b* ' )  %These  were  all  used 

%  make  graphs  for  real  data  and  for  simulation  two 
plot (node_pos ( : , 1 ) , node_pos ( : , 2 ) , ' ks ' ) 
plot (xogab, yogab,  ' ro ' ,  'MarkerSize '  ,  5) 
plot (xgood, ygood,  ' ro ' ,  'MarkerSize '  ,  10) 


%  error  (g)  =sqrt  (  (xf  inal2  (g)  -glider_pos  (g,  1)  )  ''2  +  (yfinal2  (g)  -  .  .  . 

%  glider_pos (g, 2 ) ) ^2 ) ;  %Used  in  simulations 

end  %This  closes  the  primary  loop 
%  plot (xf inal2 ,  yf inal2 ,' r+ MarkerSize ', 10 ) 

%  plot (gps ( : , 1) , gps ( : , 2) , ' r- . ' ) 

fix  pos  =  [xfinal2;  yfinal2] ' ; 

%  pos_file  =  /simulations/sean_pos_out_a095b0 . txt ') ; 

%  out_fid  =  fopen (pos_f ile,  'w'); 

%  write_count  =  fprintf (out_fid,  ' %g  %g\n',  fix_pos'); 

%  st  =  fclose (out_f id) ; 

g, 

o 

%  ave  error=mean (error) 

%  sd_error=std (error ) 

%  max_error=max (error) 

g, 

o 

%  figure (2 ) 

%  hist (error, 20) 

%  sorted_error=sort (error) ;  %Sorts  all  error  values 

%  percentSO  error=sorted_error ( . 5*G)  %These  give  the  percentiles  of 
error 

%  percent75  error=sorted_error ( . 75*G)  %for  the  simulations 
%  percentOO  error=sorted_error ( . 9*G) 

%  percent95_error=sorted_error ( . 95*G) 
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%  percent99_error=sorted_error ( . 99*G) 


2,  Weighting  Method  [2] 

function  xy  position  =  determine  pos 

%ENS  Matthew  Hahn 
%6  MAY  2005 

%this  version  of  the  algorithm  takes  in  an  Excel  data  file  with  the 
ranges  from  each 

%time  sample  and  loops  through  the  rows  to  garner  a  solution  at  each 
point  in  time 

global  all_nodes  range_data; 

%  close  all 
%  clear  all 
%  clc 

%  non  mobile_nodes_10  May=xlsread (' fixed  node  positions'); 

%  pings_ranges=xlsread ( ' j uly2 1  ping_ranges ' ) ; 

0, 

o 

%  Lat=zeros (size (non  mobile_nodes  1 0_May, 1 ) , 1 ) ; 

%  Long=zeros (size (non_mobile  nodes_10_May, 1 ) , 1 ) ; 

g, 

o 

%  for (k=l : 1 : size (non_mobile  nodes_l 0_May, 1 ) ) 

%  Lat(k)=non  mobile_nodes_l 0_May ( k, 1 )  + 

(non_mobile_nodes_l 0_May ( k, 2 ) / 60 ) ; 

%  Long ( k) = (non_mobile  nodes_l 0_May ( k, 3 ) - 

(non_mobile_nodes_l 0_May ( k,  4 ) / 60 )  )  ; 

%  end 

g, 

o 

%  %we  establish  the  first  node  (RIO)  as  the  origin  and  measure  all 
other  node  locations 
%  %relative  to  it 

%  node_positions=zeros (size (non_mobile_nodes_10_May, 1) ,2) ; 

%  node_positions ( 1 ,  : )  =  [ 0  0]; 

%  for (k=2 : 1 : size (non  mobile_nodes_l 0_May, 1 ) ) 

%  node_positions (k, 2) = (Lat (k) -Lat (1) ) *111325;  %1  degree  latitude  is 

equal  to  111km 

%  node_positions (k, 1) = (Long (k) - 

Long (1) ) *111325*cos (Lat (1) * (pi/180) ) ; 

%  end 

node  positions  =  all  nodes; 
pings  ranges  =  range  data'; 

n  xy=node  positions; 

%it  is  useful  to  create  a  matrix  containing  all  inter-node  ranges 
%so  that  they  will  be  available  for  future  calculations 
range_set=zeros (size (n_xy, 1) , size (n_xy, 1) ) ; 
for (i=l : 1 : size (n  xy,l)) 

for(j=l:l:size (n_xy, 1) ) 
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end 


if (i~=j ) 

X  diff=n  xy(i,l)-n  xy(j,l); 

y_dif f=n_xy ( i , 2 ) -n_xy ( j , 2 ) ; 

range_set ( i , j ) =sqrt (x_dif f ^2+y_dif f ^2 ) ; 

range_set ( j , i) =range_set (i,  j )  ; 

end 


end 

%we  now  import  the  Excel  file  which  has  the  range  data  for  each  node  at 
each 

%time  sample  (rows  represent  times,  column  numbers  =  node  numbers) 

%  ranges=pings_ranges (24 : 32  ,  : ) ; 
ranges=pings_ranges (:,:); 

%now  we  begin  the  primary  loop,  which  estimates  the  position  of  the 
mobile  node 

%for  each  set  of  sampled  data  (assume  every  5  min  for  now,  record  time 
stamps  for 

%further  analysis  later) 
for(a=l:l:size (ranges, 1) ) 

clear  pairs  R_1  R_2  R_3  theta_a  org  xax  L_o_x  delta_x  delta_y  phi 
phi_prime 

clear  theta  gamma  a  gamma_b  x  y  W  alpha  x_diff  y_diff  x_fin  y_fin 
%  figure (a) 

g, 

o 

%  hold  on 

%  %now  we  need  to  compute  ranges  from  each  fixed  node  to  the  mobile 
node 

%  for (i=l : 1 : size (n_xy, 1) ) 

%  %now  draw  a  range  circle  from  the  fixed  node  in  consideration 

%  %non-available  ranges  given  a  value  >=  lO'^lO,  so  we  do  not  plot 

%  %ranges  circles  of  magnitude  greater  than  or  equal  to  lO'^lO 

%  if  (ranges  (a,  i)  <=  (lO'^lO)  ) 

%  plot (n_xy ( : , 1 ) , n_xy ( :  ,  2 )  ,  '  ks  '  ) 

%  draw_circle (ranges (a, i) , n_xy (i, 1) , n_xy (i, 2) ) ; 

%  end 

%  end 


%  grid 

%for(i=l:l:size (n_xy,  1) ) 

%  text (n_xy (i, 1) +15, n_xy (i, 2) +15,  num2str (i  +  9)  )  ; 

%end 

%now  that  we  have  all  possible  node  pairs  (from  range  set),  we 
%determine  which  ones  will  yield  a  real  solution  for  mobile  node 
position 

pairs=zeros (size (range_set, 1) , size (range_set, 2) ) ; 
for(i=l:l:size( pairs, 1) ) 

for(j=l:l:size( pairs, 1) ) 

if (i~=j) 

R_l=ranges (a, i) ; 

R_2=ranges (a, j ) ; 

R_3=range_set (i, j ) ; 
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theta  a=cosines  law(R  1,R  3,R  2); 
if (imag (theta_a) ==0) 
pairs (i, j ) =1; 
pairs ( j , i) =pairs (i, j ) ; 

end 

end 

end 

end 

%now  we  determine  the  xy  coordinates  of  the  two  possible  solutions  for 
each  pair 

%that  yields  a  real  solution  for  theta 
c=l; 

for(i=l:l:size( pairs, 1)  ) 

for(j=i:l:size( pairs, 1) ) 
if (pairs (i, j ) ==1) 

org= [n_xy (i, 1)  n_xy(i,2)]; 
xax= [n_xy ( j , 1)  n_xy(j,2)]; 

L_o_x=range_set (i,  j )  ; 
delta_x=xax (1,1) -org (1,1); 
delta_y=xax (1,2) -org (1,2)  ; 

if (delta  x==0)  %case  of  an  infinite  value  of  inverse 

tangent 

if (org ( 1 , 2 ) >xax (1,2) ) 
phi=-pi/2 ; 

else 

phi=pi/2; 

end 

else 

phi_prime=atan (abs (delta_y) /abs (delta_x) ) ; 
if ( (org ( 1 , 1 ) >xax (l,l))&(org(l,2) >xax (1,2) ) ) 
phi=phi  prime+pi;  %3rd  quadrant 
elseif ( (org(l, 1) >xax (l,l))&(org(l,2) <xax (1,2) ) ) 
phi=pi-phi  prime;  %2nd  quadrant 
elseif ( (org(l, 1) <xax (l,l))&(org(l,2) >xax (1,2)  )  ) 
phi=2*pi-phi  prime;  %4th  quadrant 
elseif ( (org(l, 1) <xax (l,l))&(org(l,2) <xax (1,2) ) ) 
phi=phi  prime; 

end 

end 

%find  abs  value  of  angle  of  solution  offset  from  new  xaxis 
%this  is  easily  determined  using  the  law  of  cosines, 

%where  "opp  side"  (see  function  code)  is  range:  xax  to 

mobile  node 

theta=cosines_law (ranges (a, i) , L_o_x, ranges (a,  j ) ) ; 

%soln  will  be  endpoint  of  vector:  range  from  origin, 

+origin 

%coordinates ,  angle  equal  to  gamma  (a  or  b)  calculated 

already 

gamma  a=phi+theta; 
gamma  b=phi-theta; 


X (c, 1) =org (1,1) iranges (a, i) *cos (gamma_a) ; 

X (c+1, 1) =org (1,1) franges (a, i) *cos (gamma_b) ; 
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y  (c,  1)  =org  (1, 2)  +  ranges  (a,  i)  *sin  (gamma_a)  ; 
y(c+l,l)=org(l,2) +ranges (a, i) *sin (gamma_b) ; 
c=c+2 ; 

end 

end 

end 

%calculate  weight  values... 

W=zeros (size (x, 1 ) , 1 ) ; 
for(i=l:l:size(x,l) ) 

for(j=l:l:size(x,l) ) 

if (j~=i) 

alpha=2 ; 

x_dif f=x ( i ) -X ( j ) ; 
y_diff=y (i) -y ( j ) ; 

W(i)=W(i)+  (x  diff^2+y  dif f ^2 ) ^-alpha; 

end 

end 

end 

W  sum=sum (W) ; 

for(i=l:l:size(W,l) ) 

X  f in ( i ) = ( (W ( i ) /W  sum) *x ( i ) ) ; 
y_f in ( i ) = ( (W ( i ) /W_sum) *y ( i ) ) ; 

end 

X  position ( 1 , a) =sum (x  fin); 
y_position ( 1 , a) =sum (y_f in) ; 

%  latitude_soln (1, a) = (x_position (1, a) /111325) +Lat (1) ; 

q, 

o 

longitude_soln (1, a) = (y_position (l,a) / (111325*cos (Lat (1) *  (pi/180) ) ) ) +Lon 

g  ( 1 ) ; 

%  hold  on 
%  %plot (x, y, ' r* ' ) 

%  %plot (x_position, y_position,  'g:  +  ') 

%  plot  (x_position  (a)  ,  y_position  (a)  ,  '  r''  '  ) 

%  hold  off 


%pause (0.5) 
end 

xy_position  =  [x_position;  y_position] ' ; 

%  figure (a+1 ) 

%  hold  on 

%  plot (n_xy ( : , 1) , n_xy ( :  ,  2)  ,  ' k* ' ) ; 

%  plot (x_position, y_position,  'g:  +  ') 

%  for (i=l : 1 : size (n_xy, 1) ) 

%  text (n_xy (i, 1) , n_xy (i, 2) , num2str (i  +  8) )  ; 
%  end 
%  grid 

%  x_position; 
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o\°  o\°  o\° 


y_position; 
latitude_soln; 
longitude  soln; 
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